def __init__(self):
        rospy.init_node('gripper_controller')
        self.msg = None
       
       
        self.r_pub = rospy.Publisher('gripper_controller/command',
                Float64,
                queue_size=10)
        """
        self.t_pub = rospy.Publisher('arm_controller/command',
                JointTrajectory,
                queue_size=10)
        self.js_sub = rospy.Subscriber('joint_states', JointState,
                self._jointStateCb)
        """

        # subscribe to command and then spin
        self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
        self.server.start()
        rospy.spin() 

def scaleWheel():
    rospy.init_node("wheel_scaler")
    rospy.loginfo("wheel_scaler started")
    
    scale = rospy.get_param('distance_scale', 1)
    rospy.loginfo("wheel_scaler scale: %0.2f", scale)
    
    rospy.Subscriber("lwheel", Int16, lwheelCallback)
    rospy.Subscriber("rwheel", Int16, rwheelCallback)
    
    lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10)
    rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10) 
    
    ### testing sleep CPU usage
    r = rospy.Rate(50) 
    while not rospy.is_shutdown:
        r.sleep()
        
    rospy.spin() 

def start(self, node_name='polly_node', service_name='polly'):
        """The entry point of a ROS service node.

        Details of the service API can be found in Polly.srv.

        :param node_name: name of ROS node
        :param service_name:  name of ROS service
        :return: it doesn't return
        """
        rospy.init_node(node_name)

        service = rospy.Service(service_name, Polly, self._node_request_handler)

        rospy.loginfo('polly running: {}'.format(service.uri))

        rospy.spin() 

def __init__(self, args):
        self.index = 0
        if len(sys.argv) > 1:
            self.index = int(sys.argv[1])
        rospy.init_node('save_img')
        self.bridge = CvBridge()

        while not rospy.is_shutdown():
            raw_input()

            data = rospy.wait_for_message('/camera1/usb_cam/image_raw', Image)

            try:
                cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
            except CvBridgeError as e:
                print(e)

            print "Saved to: ", base_path+str(self.index)+".jpg"
            cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB, cv_image)
            cv2.imwrite(join(base_path, "frame{:06d}.jpg".format(self.index)), cv_image)#*255)
            self.index += 1
        rospy.spin() 

def main_sawyer():
    import intera_interface
    from visual_mpc.envs.robot_envs.sawyer.sawyer_impedance import SawyerImpedanceController
    
    controller = SawyerImpedanceController('sawyer', True, gripper_attached='none')       # doesn't initial gripper object even if gripper is attached

    def print_eep(value):
        if not value:
            return
        xyz, quat = controller.get_xyz_quat()
        yaw, roll, pitch = [np.rad2deg(x) for x in controller.quat_2_euler(quat)]
        logging.getLogger('robot_logger').info("XYZ IS: {}, ROTATION IS: yaw={} roll={} pitch={}".format(xyz, yaw, roll, pitch))
    
    navigator = intera_interface.Navigator()
    navigator.register_callback(print_eep, 'right_button_show')
    rospy.spin() 

def listener():

	global obj
	global pub

	rospy.loginfo("Starting prediction node")

	rospy.init_node('listener', anonymous = True)

	rospy.Subscriber("sensor_read", Int32, read_sensor_data)

	pub = rospy.Publisher('predict', Int32, queue_size=1)


	obj = Classify_Data()

	obj.read_file('pos_readings.csv')

	obj.train()

	rospy.spin() 

def run(args=None):    
    parser = OptionParser(description='Crazyflie ROS Driver')
#     parser.add_option('--uri', '-u',
#                         dest='uri',
#                         default='radio://0/4',
#                         help='URI to connect to')    
    (options, leftargs) = parser.parse_args(args=args)
        
    #Load some DB depending on options 
    rospy.init_node('CrazyFlieJoystickDriver')   
    joydriver = JoyController(options)

    #START NODE
    try:
        rospy.loginfo("Starting CrazyFlie joystick driver")
        rospy.spin()
    except KeyboardInterrupt:    
        rospy.loginfo('...exiting due to keyboard interrupt') 

def run(args=None):    
    parser = OptionParser(description='Crazyflie ROS Driver')
#     parser.add_option('--uri', '-u',
#                         dest='uri',
#                         default='radio://0/4',
#                         help='URI to connect to')    
    (options, leftargs) = parser.parse_args(args=args)
        
    #Load some DB depending on options 
    rospy.init_node('CrazyFlieJoystickDriver')   
    joydriver = JoyController(options)

    #START NODE
    try:
        rospy.loginfo("Starting CrazyFlie joystick driver")
        rospy.spin()
    except KeyboardInterrupt:    
        rospy.loginfo('...exiting due to keyboard interrupt') 

def __init__(self):
        self.evadeSet = False
        self.controller = XBox360()
        self.bridge = CvBridge()
        self.throttle = 0
        self.grid_img = None
        ##self.throttleLock = Lock()
        print "evasion"
        rospy.Subscriber("/lidargrid", Image, self.gridCB, queue_size=1)
        rospy.Subscriber("/cmd_vel", TwistStamped , self.twistCB , queue_size = 1)
        rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5)
        self.pub_img = rospy.Publisher("/steering_img", Image)
        self.pub_twist = rospy.Publisher("/lidar_twist", TwistStamped, queue_size=1)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)        
        rospy.init_node ('lidar_cmd',anonymous=True)
        rospy.spin() 

def __init__(self):
        print "dataRecorder"
        self.record = False
        self.twist = None
        self.twistLock = Lock()
        self.bridge = CvBridge()
        self.globaltime = None
        self.controller = XBox360()
        ##rospy.Subscriber("/camera/depth/points" , PointCloud2, self.ptcloudCB)
        rospy.Subscriber("/camera/depth/image_rect_raw_drop", Image, self.depthCB)
        rospy.Subscriber("/camera/rgb/image_rect_color_drop", Image, self.streamCB)
        #rospy.Subscriber("/camera/rgb/image_rect_mono_drop", Image, self.streamCB)  ##for black and white images see run.launch and look at the drop fps nodes near the bottom
        rospy.Subscriber("/lidargrid", Image, self.lidargridCB)
        rospy.Subscriber("/cmd_vel", TwistStamped, self.cmd_velCB)
        rospy.Subscriber("/joy", Joy ,self.joyCB)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
        rospy.init_node('dataRecorder',anonymous=True)
        rospy.spin() 

def __init__(self):
        rospy.loginfo("runner.__init__")
        # ----------------------------
        self.sess = tf.InteractiveSession()
        self.model = cnn_cccccfffff()
        saver = tf.train.Saver()
        saver.restore(self.sess, "/home/ubuntu/catkin_ws/src/car/scripts/model.ckpt")
        rospy.loginfo("runner.__init__: model restored")
        # ----------------------------
        self.bridge = CvBridge()
        self.netEnable = False
        self.controller = XBox360()
        rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.runCB)
        rospy.Subscriber("/joy", Joy ,self.joyCB)
        self.pub_twist = rospy.Publisher("/neural_twist", TwistStamped, queue_size=1)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
        rospy.init_node('neural_cmd',anonymous=True)
        rospy.spin() 

def listener():
    rospy.init_node('serial_adapter', anonymous=True)
    # Declare the Subscriber to motors orders
    rospy.Subscriber("arduino/servo", Int16, servoCallback, queue_size=2)
    rospy.Subscriber("arduino/motor", Int8, motorCallback, queue_size=2)
    rospy.spin() 

def run(self):
        rospy.Rate(1)
        rospy.spin() 

def run():
        """
        This function just keeps the node alive.

        """
        rospy.spin() 

def navigation():
    rospy.Publisher('usv_position_setpoint', Odometry) # only create a rostopic, navigation system TODO
    rospy.spin()

   # while not rospy.is_shutdown():
   #     rospy.loginfo(hello_str)
   #     pub.publish(hello_str)
   #     rate.sleep() 

def start(self, node_name='synthesizer_node', service_name='synthesizer'):
        """The entry point of a ROS service node.

        :param node_name: name of ROS node
        :param service_name:  name of ROS service
        :return: it doesn't return
        """
        rospy.init_node(node_name)

        service = rospy.Service(service_name, Synthesizer, self._node_request_handler)

        rospy.loginfo('{} running: {}'.format(node_name, service.uri))

        rospy.spin() 

def start_server(limb, rate, mode, valid_limbs):
    rospy.loginfo("Initializing node... ")
    rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format(
                        mode, "" if limb == 'all_limbs' else "_" + limb,))

    rospy.wait_for_service("/ExternalTools/right/PositionKinematicsNode/IKService")

    rospy.loginfo("Initializing joint trajectory action server...")
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
    config_module = "intera_interface.cfg"
    if mode == 'velocity':
        config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"])
    elif mode == 'position':
        config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"])
    else:
        config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"])
    cfg = importlib.import_module('.'.join([config_module,config_name]))
    dyn_cfg_srv = Server(cfg, lambda config, level: config)
    jtas = []
    if limb == 'all_limbs':
        for current_limb in valid_limbs:
            jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv,
                                                    rate, mode))
    else:
        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))


    def cleanup():
        for j in jtas:
            j.clean_shutdown()

    rospy.on_shutdown(cleanup)
    rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit")
    rospy.spin() 

def start_server(gripper):
    print("Initializing node... ")
    rospy.init_node("rsdk_gripper_action_server%s" %
                    ("" if gripper == 'both' else "_" + gripper,))
    print("Initializing gripper action server...")

    gas = GripperActionServer()

    print("Running. Ctrl-c to quit")
    rospy.spin() 

def __init__(self):
        super(PeopleObjectDetectionNode, self).__init__()

        # init the node
        rospy.init_node('people_object_detection', anonymous=False)

        # Get the parameters
        (model_name, num_of_classes, label_file, camera_namespace, video_name,
            num_workers) \
        = self.get_parameters()

        # Create Detector
        self._detector = Detector(model_name, num_of_classes, label_file,
            num_workers)

        self._bridge = CvBridge()

        # Advertise the result of Object Detector
        self.pub_detections = rospy.Publisher('/object_detection/detections', \
            DetectionArray, queue_size=1)

        # Advertise the result of Object Detector
        self.pub_detections_image = rospy.Publisher(\
            '/object_detection/detections_image', Image, queue_size=1)

        if video_name == "no":
            # Subscribe to the face positions
            self.sub_rgb = rospy.Subscriber(camera_namespace,\
                Image, self.rgb_callback, queue_size=1, buff_size=2**24)
        else:
            self.read_from_video(video_name)
        # spin
        rospy.spin() 

def __init__(self):
        super(FaceRecognitionNode, self).__init__()

        # init the node
        rospy.init_node('face_recognition_node', anonymous=False)

        # Get the parameters
        (image_topic, detection_topic, output_topic, output_topic_rgb) \
            = self.get_parameters()

        self._bridge = CvBridge()

        # Advertise the result of Object Tracker
        self.pub_det = rospy.Publisher(output_topic, \
            DetectionArray, queue_size=1)

        self.pub_det_rgb = rospy.Publisher(output_topic_rgb, \
            Image, queue_size=1)


        self.sub_detection = message_filters.Subscriber(detection_topic, \
            DetectionArray)
        self.sub_image = message_filters.Subscriber(image_topic, Image)

        # Scaling factor for face recognition image
        self.scaling_factor = 1.0

        # Read the images from folder and create a database
        self.database = self.initialize_database()

        ts = message_filters.ApproximateTimeSynchronizer(\
            [self.sub_detection, self.sub_image], 2, 0.3)

        ts.registerCallback(self.detection_callback)

        # spin
        rospy.spin() 

def __init__(self):
        super(PeopleObjectTrackerNode, self).__init__()

        # init the node
        rospy.init_node('people_object_tracker', anonymous=False)

        # Get the parameters
        (detection_topic, tracker_topic, cost_threshold, \
            max_age, min_hits) = \
            self.get_parameters()

        self._bridge = CvBridge()

        self.tracker = sort.Sort(max_age=max_age, min_hits=min_hits)

        self.cost_threshold = cost_threshold

        # Advertise the result of Object Tracker
        self.pub_trackers = rospy.Publisher(tracker_topic, \
            DetectionArray, queue_size=1)

        self.sub_detection = rospy.Subscriber(detection_topic, \
            DetectionArray,\
            self.detection_callback)

        # spin
        rospy.spin() 

def __init__(self):
        super(ProjectionNode, self).__init__()

        # init the node
        rospy.init_node('projection_node', anonymous=False)

        self._bridge = CvBridge()

        (depth_topic, face_topic, output_topic, f, cx, cy) = \
            self.get_parameters()

         # Subscribe to the face positions
        sub_obj = message_filters.Subscriber(face_topic,\
            DetectionArray)

        sub_depth = message_filters.Subscriber(depth_topic,\
            Image)

        # Advertise the result of Face Depths
        self.pub = rospy.Publisher(output_topic, \
            DetectionArray, queue_size=1)

        # Create the message filter
        ts = message_filters.ApproximateTimeSynchronizer(\
            [sub_obj, sub_depth], \
            2, \
            0.9)

        ts.registerCallback(self.detection_callback)

        self.f = f
        self.cx = cx
        self.cy = cy

        # spin
        rospy.spin() 

def main_baxter(limb='right'):
    import baxter_interface
    from visual_mpc.envs.robot_envs.baxter.baxter_impedance import BaxterImpedanceController
    controller = BaxterImpedanceController('baxter', True, gripper_attached='none', limb=limb)

    def print_eep(value):
        if not value:
            return
        xyz, quat = controller.get_xyz_quat()
        yaw, roll, pitch = [np.rad2deg(x) for x in controller.quat_2_euler(quat)]
        logging.getLogger('robot_logger').info("XYZ IS: {}, ROTATION IS: yaw={} roll={} pitch={}".format(xyz, yaw, roll, pitch))
    
    navigator = baxter_interface.Navigator(limb)
    navigator.button0_changed.connect(print_eep)
    rospy.spin() 

def main_franka():
    from visual_mpc.envs.robot_envs.franka.franka_impedance import FrankaImpedanceController
    controller = FrankaImpedanceController('franka', True, gripper_attached='hand')       # doesn't initial gripper object even if gripper is attached

    def print_eep(value):
        if not value:
            return
        xyz, quat = controller.get_xyz_quat()
        yaw, roll, pitch = [np.rad2deg(x) for x in controller.quat_2_euler(quat)]
        print(xyz)
        logging.getLogger('robot_logger').info("XYZ IS: {}, ROTATION IS: yaw={} roll={} pitch={}".format(xyz, yaw, roll, pitch))
    
    print_eep(1.0)
    rospy.spin() 

def start(self):
        """Start the dialogflow client"""
        rospy.loginfo("DF_CLIENT: Spinning...")
        rospy.spin() 

def listener():

	rospy.loginfo("Starting listening to response")
	rospy.Subscriber("response",String, get_response,queue_size=10)
	rospy.spin() 

def listener():
	rospy.loginfo("Starting Speech Recognition")
	rospy.Subscriber("/recognizer/output", String, get_speech)
	rospy.spin() 

def listener():

	rospy.loginfo("Starting ROS AIML Server")
	rospy.Subscriber("chatter", String, callback)
    
	# spin() simply keeps python from exiting until this node is stopped
	rospy.spin() 

def main(self):
        rospy.spin() 

def main():
    p = CokeCanPickAndPlace()

    rospy.spin()