Python rospy 模块，init_node() 实例源码

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

#    rate = rospy.Rate(10)
#    hello_str = "hello world"
 #   rospy.loginfo(hello_str)
 #   pub.publish(hello_str)
 #   rospy.spin()
 #   exit(0)

def run_driver():
    # init moveit commander
    moveit_commander.roscpp_initialize(sys.argv)
    # specify move group
    group = moveit_commander.MoveGroupCommander("arm")
    # init ros node
    rospy.init_node('real_servo_driver', anonymous=True)

    print "============ Waiting for RVIZ..."
    rospy.sleep(2)
    # move grasper to init position
    init_position(group)

    # set ros publisher rate, 10hz = 10 seconds for a circle
    rate = rospy.Rate(50)
    while True:
        group.set_random_target()
        plan_msg = group.plan()
        group.execute(plan_msg=plan_msg, wait=False)
        rospy.sleep(5)
        if is_exit:
            break
    # shutdown moveit commander
    moveit_commander.roscpp_shutdown()

def main():
    rospy.init_node("whatsapp_service")
    cred = credentials.WHATSAPP
    stackBuilder = YowStackBuilder()
    stack = (stackBuilder
             .pushDefaultLayers(True)
             .push(AideRosLayer)
             .build())
    loginfo("Stack built...")
    stack.setCredentials(cred)
    stack.broadcastEvent(YowLayerEvent(YowNetworkLayer.EVENT_STATE_CONNECT))  # sending the connect signal
    loginfo("Connected...")
    atexit.register(lambda: stack.broadcastEvent(YowLayerEvent(YowNetworkLayer.EVENT_STATE_DISCONNECT)))

    th = threading.Thread(target=stack.loop)
    th.daemon = True
    th.start()
    loginfo("Running in background.")
    loginfo("All done. spinning.")
    while not rospy.is_shutdown():
        rospy.spin()

def main():
    rospy.init_node("actions")
    loginfo("Creating action handler...")
    action_handler = ActionHandler()
    loginfo("Registering services...")

    get_service_handler(CallFunction).register_service(
        lambda func_name, kwargs: action_handler.call_func(func_name, **json.loads(kwargs))
    )
    rospy.Subscriber("/aide/update_apis", String, lambda x: action_handler.reload_api_references(x.data))

    get_service_handler(GetActionProvider).register_service(lambda name: action_handler.get_action_provider(name) or ())
    get_service_handler(GetAllActionProviders).register_service(action_handler.get_all_action_providers)
    get_service_handler(AddActionProvider).register_service(action_handler.add_action_provider)
    get_service_handler(DeleteActionProvider).register_service(action_handler.remove_action_provider)

    loginfo("Registered services. Spinning.")

    rospy.spin()

def main():
    rospy.init_node("api_handler")
    loginfo("Creating api handler...")
    notify_publisher = rospy.Publisher("/aide/update_apis", String, queue_size=50)

    api = ApiHandler(lambda x: notify_publisher.publish(x))
    loginfo("Registering services...")

    get_service_handler(GetApi).register_service(lambda **args: api.get_api(**args) or ())
    get_service_handler(GetAllApis).register_service(api.get_all_apis)
    get_service_handler(AddApi).register_service(api.add_api)
    get_service_handler(DeleteApi).register_service(api.remove_api)

    loginfo("Registered services. Spinning.")

    rospy.spin()

def init(self):
        pygame.init()
        clock = pygame.time.Clock()
        screen = pygame.display.set_mode((250, 250))

        rospy.init_node('highway_teleop')
        self.rate = rospy.Rate(rospy.get_param('~hz', 10)) 
        self.acc = rospy.get_param('~acc', 5)
        self.yaw = rospy.get_param('~yaw', 0)

        self.robot_pub = rospy.Publisher('robot_0/cmd_vel', Twist, queue_size=1)

        print "Usage: \n \
                up arrow: accelerate \n \
                down arrow: decelerate \n \
                left arrow: turn left \n \
                right arrow: turn right"

def init(self):
        pygame.init()
        clock = pygame.time.Clock()
        screen = pygame.display.set_mode((250, 250))

        rospy.init_node('teleop')
        self.rate = rospy.Rate(rospy.get_param('~hz', 20)) 
        self.acc = rospy.get_param('~acc', 1)
        self.yaw = rospy.get_param('~yaw', 0.25)

        self.robot_pub = rospy.Publisher('control_command', controlCommand, queue_size=1)

        self.path_record_pub = rospy.Publisher('record_state', \
                RecordState, queue_size=1)

        self.highway_game_start_pub = rospy.Publisher('highway_game_start', RecordState, queue_size=1)

        print "Usage: \n \
                up arrow: accelerate \n \
                down arrow: decelerate \n \
                left arrow: turn left \n \
                right arrow: turn right"

def main():
    rospy.init_node("listener")

    sub = rospy.Subscriber("/chatter_topic", String, callback)

    rospy.spin()

def main(args):
    rospy.init_node("publish_calib_file", args)

    files = glob.glob("left-*.png");
    files.sort()

    print("All {} files".format(len(files)))

    image_pub = rospy.Publisher("image",Image, queue_size=10)
    bridge = CvBridge();


    for f in files:
        if rospy.is_shutdown():
            break
        raw_input("Publish {}?".format(f))
        img = cv2.imread(f, 0)
        image_pub.publish(bridge.cv2_to_imgmsg(img, "mono8"))

def main():
    from optparse import OptionParser
    rospy.init_node('cameracheck')
    parser = OptionParser()
    parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]")
    parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]")
    options, args = parser.parse_args()
    size = tuple([int(c) for c in options.size.split('x')])
    dim = float(options.square)
    CameraCheckerNode(size, dim)
    rospy.spin()

def main():
    rospy.init_node('easy_handeye')
    while rospy.get_time() == 0.0:
        pass

    print('Handeye Calibration Commander')
    print('connecting to: {}'.format((rospy.get_namespace().strip('/'))))

    cmder = HandeyeCalibrationCommander()

    if cmder.client.eye_on_hand:
        print('eye-on-hand calibration')
        print('robot effector frame: {}'.format(cmder.client.robot_effector_frame))
    else:
        print('eye-on-base calibration')
        print('robot base frame: {}'.format(cmder.client.robot_base_frame))
    print('tracking base frame: {}'.format(cmder.client.tracking_base_frame))
    print('tracking target frame: {}'.format(cmder.client.tracking_marker_frame))

    cmder.spin_interactive()

def main():
    rospy.init_node('easy_handeye')
    while rospy.get_time() == 0.0:
        pass

    cw = HandeyeServer()

    rospy.spin()

def main():
    rgb_object_detection = RGBObjectDetection()
    rospy.init_node('rgb_object_detection', anonymous=True)
    try:
      rospy.spin()
    except KeyboardInterrupt:
      print("Shutting down")

def main():
    store_data = StoreData()
    rospy.init_node('store_data', anonymous=True)
    try:
        rospy.spin()
    except KeyboardInterrupt:
      print("Shutting down")

def main():
    """SDK Navigator Example

    Demonstrates Navigator by echoing input values from wheels and
    buttons.

    Uses the intera_interface.Navigator class to demonstrate an
    example of using the register_callback feature.

     Shows Navigator input of the arm for 10 seconds.
    """
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument(
        "-n", "--navigator", dest="nav_name", default="right",
        choices=["right", "head"],
        help='Navigator on which to run example'
        )
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('sdk_navigator', anonymous=True)
    echo_input(args.nav_name)
    return 0

def main():
    """Intera RSDK Joint Velocity Example: Wobbler

    Commands joint velocities of randomly parameterized cosine waves
    to each joint. Demonstrates Joint Velocity Control Mode.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_joint_velocity_wobbler")

    wobbler = Wobbler()
    rospy.on_shutdown(wobbler.clean_shutdown)
    wobbler.wobble()

    print("Done.")

def main():
    """RSDK Inverse Kinematics Example

    A simple example of using the Rethink Inverse Kinematics
    Service which returns the joint angles and validity for
    a requested Cartesian Pose.

    Run this example, the example will use the default limb
    and call the Service with a sample Cartesian
    Pose, pre-defined in the example code, printing the
    response of whether a valid joint solution was found,
    and if so, the corresponding joint angles.
    """
    rospy.init_node("rsdk_ik_service_client")

    if ik_service_client():
        rospy.loginfo("Simple IK call passed!")
    else:
        rospy.logerr("Simple IK call FAILED")

    if ik_service_client(use_advanced_options=True):
        rospy.loginfo("Advanced IK call passed!")
    else:
        rospy.logerr("Advanced IK call FAILED")

def main():
    """RSDK Forward Kinematics Example

    A simple example of using the Rethink Forward Kinematics
    Service which returns the Cartesian Pose based on the input joint angles.

    Run this example, the example will use the default limb
    and call the Service with a sample Joint Position,
    pre-defined in the example code, printing the
    response of whether a valid Cartesian solution was found,
    and if so, the corresponding Cartesian pose.
    """
    rospy.init_node("rsdk_fk_service_client")

    if fk_service_client():
        rospy.loginfo("Simple FK call passed!")
    else:
        rospy.logerr("Simple FK call FAILED")

def main():
    """RSDK Head Example: Wobbler

    Nods the head and pans side-to-side towards random angles.
    Demonstrates the use of the intera_interface.Head class.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("rsdk_head_wobbler")

    wobbler = Wobbler()
    rospy.on_shutdown(wobbler.clean_shutdown)
    print("Wobbling... ")
    wobbler.wobble()
    print("Done.")

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.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 main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-t", "--timeout", type=lambda t:abs(float(t)),
            default=60.0, help="[in seconds] Time to wait for joints to home")
    parser.add_argument("-m", "--mode", type=str.upper, default="AUTO",
            choices=['AUTO', 'MANUAL'], help="Mode to home the robot's joints")
    enable_parser = parser.add_mutually_exclusive_group(required=False)
    enable_parser.add_argument("-e", "--enable", action='store_true', dest='enable',
                       help="Try to enable the robot before homing.")
    enable_parser.add_argument("-n", "--no-enable", action='store_false', dest='enable',
                       help="Avoid trying to enable the robot before homing.")
    enable_parser.set_defaults(enable=True)
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('home_joints_node')
    if args.enable:
        rs = intera_interface.RobotEnable(CHECK_VERSION)
        rs.enable()
    cmd_mode = HomingCommand.MANUAL if args.mode == 'MANUAL' else HomingCommand.AUTO
    rospy.loginfo("Homing joints in '{0}' mode".format(args.mode.capitalize()))
    home_jnts = HomeJoints()
    state = home_jnts.home_robot(mode=cmd_mode, timeout=args.timeout)
    rospy.loginfo("{0} in homing the robot's joints".format("Succeeded" if state else "Failed"))

def start():
    global left_motor_pub,right_motor_pub
    # publishing to "turtle1/cmd_vel" to control turtle1
    global pub
    pub = rospy.Publisher('turtle1/cmd_vel', Twist)
    left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
    right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
    # subscribed to joystick inputs on topic "joy"
    rospy.Subscriber("joy", Joy, callback)
    # starts the node
    rospy.init_node('Joy2Turtle')
    rospy.spin()

def start():
    global left_motor_pub,right_motor_pub
    # publishing to "turtle1/cmd_vel" to control turtle1
    global pub
    pub = rospy.Publisher('turtle1/cmd_vel', Twist)
    left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
    right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
    # subscribed to joystick inputs on topic "joy"
    rospy.Subscriber("joy", Joy, callback)
    # starts the node
    rospy.init_node('Joy2Turtle')
    rospy.spin()

def start():
    global left_motor_pub,right_motor_pub
    # publishing to "turtle1/cmd_vel" to control turtle1
    global pub
    pub = rospy.Publisher('turtle1/cmd_vel', Twist)
    left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
    right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
    # subscribed to joystick inputs on topic "joy"
    rospy.Subscriber("joy", Joy, callback)
    # starts the node
    rospy.init_node('Joy2Turtle')
    rospy.spin()

def start():
    global left_motor_pub,right_motor_pub
    # publishing to "turtle1/cmd_vel" to control turtle1
    global pub
    pub = rospy.Publisher('turtle1/cmd_vel', Twist)
    left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
    right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
    # subscribed to joystick inputs on topic "joy"
    rospy.Subscriber("joy", Joy, callback)
    # starts the node
    rospy.init_node('Joy2Turtle')
    rospy.spin()

def avoid_runaway_suppressor():
    rospy.init_node('avoid_runaway_suppressor')
    rospy.Subscriber("/avoid/suppressor/heading", Heading, avoid_headingCB) 
    rospy.Subscriber("/runaway/suppressor/heading", Heading, runaway_headingCB)
    rospy.spin()

def start_detect(self):
        FunctionUnit.init_node(self)
        #print 'hello 1'
        #print self._receive_topic
        receive_1 = FunctionUnit.create_receive(self, self._receive_topic, Image, self.receive_1_cb)
        #print 'hello 2'
        FunctionUnit.spin(self)

def run(self):
        rospy.init_node('hello2')
        print 'hello2'
        rospy.spin()

def __init__(self):
        rospy.init_node('gaze', anonymous=False)

        self.lock = threading.RLock()
        with self.lock:
            self.current_state = GazeState.IDLE
            self.last_state = self.current_state

        # Initialize Variables
        self.glance_timeout = 0
        self.glance_timecount = 0
        self.glance_played = False

        self.idle_timeout = 0
        self.idle_timecount = 0
        self.idle_played = False


        rospy.loginfo('\033[92m[%s]\033[0m waiting for bringup social_mind...'%rospy.get_name())
        rospy.wait_for_service('environmental_memory/read_data')
        rospy.wait_for_service('social_events_memory/read_data')

        self.rd_memory = {}
        self.rd_memory['environmental_memory'] = rospy.ServiceProxy('environmental_memory/read_data', ReadData)
        self.rd_memory['social_events_memory'] = rospy.ServiceProxy('social_events_memory/read_data', ReadData)

        rospy.Subscriber('raising_events', RaisingEvents, self.handle_raising_events)
        rospy.Subscriber('gaze_focusing', String, self.handle_gaze_focusing)
        self.pub_gaze_cmd = rospy.Publisher('gaze_command', GazeCommand, queue_size=10)
        self.pub_viz_gaze_cmd = rospy.Publisher('visualization_gaze_cmd', PointStamped, queue_size=10)

        rospy.Timer(rospy.Duration(GAZE_CONTROLLER_PERIOD), self.handle_gaze_controller)
        rospy.loginfo('\033[92m[%s]\033[0m initialized...'%rospy.get_name())
        rospy.spin()

def __init__(self):
        rospy.init_node('fake_renderer', anonymous=True)

        try:
            topic_name = rospy.get_param('~action_name')
        except KeyError as e:
            print('[ERROR] Needed parameter for (topic_name)...')
            quit()

        if 'render_gesture' in rospy.get_name():
            self.GetInstalledGesturesService = rospy.Service(
                "get_installed_gestures",
                GetInstalledGestures,
                self.handle_get_installed_gestures
            )

            self.motion_list = {
                'neutral': ['neutral_motion1'],
                'encourge': ['encourge_motion1'],
                'attention': ['attention_motion1'],
                'consolation': ['consolation_motion1'],
                'greeting': ['greeting_motion1'],
                'waiting': ['waiting_motion1'],
                'advice': ['advice_motion1'],
                'praise': ['praise_motion1'],
                'command': ['command_motion1'],
            }

        self.server = actionlib.SimpleActionServer(
            topic_name, RenderItemAction, self.execute_callback, False)
        self.server.start()

        rospy.loginfo('[%s] initialized...' % rospy.get_name())
        rospy.spin()

def main():
    rospy.init_node("trajectory_planner")
    planner = TrajectoryPlanner()
    rospy.spin()

def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing");

def __init__( self ):
        #Initialization
        NaoqiNode.__init__( self, self.NODE_NAME )

        from distutils.version import LooseVersion
        if self.get_version() < LooseVersion('2.0.0'):
            rospy.loginfo('The NAOqi version is inferior to 2.0, hence no log bridge possible')
            exit(0)

        rospy.init_node( self.NODE_NAME )

        # the log manager is only avaiable through a session (NAOqi 2.0)
        import qi
        self.session = qi.Session()
        self.session.connect("tcp://%s:%s" % (self.pip, self.pport))
        self.logManager = self.session.service("LogManager")

        self.listener = self.logManager.getListener()
        self.listener.onLogMessage.connect(onMessageCallback)
        rospy.loginfo('Logger initialized')

def __init__(self, name):
        rospy.init_node(name)

        self.start_rotation = None

        self.WIDTH = 320
        self.HEIGHT = 240

        self.set_x_range(-30.0, 30.0)
        self.set_y_range(-30.0, 30.0)
        self.set_z_range(-30.0, 30.0)

        self.completed_image = None

        if not rospy.has_param('/ihmc_ros/robot_name'):
            rospy.logerr("Cannot run team_zarj_main.py, missing parameters!")
            rospy.logerr("Missing parameter '/ihmc_ros/robot_name'")

        else:
            self.zarj = ZarjOS()

def __init__(self):
        #init code
        rospy.init_node("robotGame")
        self.currentDist = 1
        self.previousDist = 1
        self.reached = False
        self.tf = TransformListener()

        self.left_joint_names = ['yumi_joint_1_l', 'yumi_joint_2_l', 'yumi_joint_3_l', 'yumi_joint_4_l', 'yumi_joint_5_l', 'yumi_joint_6_l', 'yumi_joint_7_l', 'gripper_l_joint']
        self.right_joint_names = ['yumi_joint_1_r', 'yumi_joint_2_r', 'yumi_joint_3_r', 'yumi_joint_4_r', 'yumi_joint_5_r', 'yumi_joint_6_r', 'yumi_joint_7_r', 'gripper_r_joint']
        self.left_positions = [-2.01081820427463881, 1.4283937236421274, -1.3593418228836045, -0.19315625641494183, 1.7016501799872579, 0.6573540231496411, 3.404315594906305, 0.0]
        self.right_positions = [0.01081820427463881, 2.4283937236421274, 0.3593418228836045, -0.19315625641494183, 1.7016501799872579, 0.6573540231496411, 3.404315594906305, 1.8145107750466565]
        self.rjv = []
        self.ljv = []

        self.pub = rospy.Publisher('my_joint_states', JointState, queue_size=1) 
        self.js = JointState()
        self.js.header = Header()
        self.js.name = self.left_joint_names + self.right_joint_names
        self.js.velocity = []
        self.js.effort = []
        self.sub = rospy.Subscriber('/joint_states', JointState, self.jsCB)
        self.destPos = np.random.uniform(0,0.25, size =(3))
        self.reset()

def main():
    rospy.init_node("evaluation_ac")
    ac = ACControllerSimulator()
    rospy.Subscriber("/aide/ac_control", Bool, callback=lambda msg: ac.increase_ac() if msg.data else ac.decrease_ac())

    console = Console()
    console.preprocess = lambda source: source[3:]
    atexit.register(loginfo, "Going down by user-input.")
    ac_thread = Thread(target=ac.simulate)
    ac_thread.daemon = True
    pub_thread = Thread(target=publish, args=(ac, ))
    pub_thread.daemon = True
    pub_thread.start()
    while not rospy.is_shutdown():
        try:
            command = console.raw_input("ac> ")
            if command == "start":
                ac_thread.start()
            if command == "end":
                return

        except EOFError:
            print("")
            ac.finished = True
            rospy.signal_shutdown("Going down.")

def __init__(self, loop_time = 0.1):
        # super init
        threading.Thread.__init__(self)
        self.name = str(self.__class__).split(".")[-1].replace("'>", "")
        signal.signal(signal.SIGINT, self.shutdown_handler)
        # print self.__class__
        # print "self.name", self.name
        # 20170314: remove this from base smp_thread because is is NOT ros
        # rospy.init_node(self.name, anonymous=True)
        # rospy.init_node(self.name, anonymous=False)
        # initialize pub sub
        self.pub_sub()
        # initialize services
        self.srv()

        self.isrunning = True
        self.cnt_main = 0
        self.loop_time = loop_time

def __init__(self, loop_time = 0.1, pubs = {}, subs = {}):
        """init args: pubs: dict with topic / [type,], subs: dict with topic / [type, callback]"""
        smp_thread.__init__(self, loop_time = loop_time)
        # now init ros node
        rospy.init_node(self.name, anonymous=True)
        # loop frequency / sampling rate
        self.rate = rospy.Rate(1./self.loop_time)
        # local pub / sub
        self.default_queue_size_pub = 2
        self.default_queue_size_sub = 2
        if len(pubs) == 0 and len(subs) == 0:
            self.pub_sub_local_legacy()
        else:
            self.pub_sub_local(pubs, subs)
        # print "smp_thread_ros pubs", self.pub
        # print "smp_thread_ros subs", self.sub

def main():
    """RSDK Forward Kinematics Example

    A simple example of using the Rethink Forward Kinematics
    Service which returns the Cartesian Pose based on the input joint angles.

    Run this example, the example will use the default limb
    and call the Service with a sample Joint Position,
    pre-defined in the example code, printing the
    response of whether a valid Cartesian solution was found,
    and if so, the corresponding Cartesian pose.
    """
    rospy.init_node("rsdk_fk_service_client")

    if fk_service_client():
        rospy.loginfo("Simple FK call passed!")
    else:
        rospy.logerror("Simple FK call FAILED")

def execute():

    # define publisher and its topic 
    pub = rospy.Publisher('write_angles',Angles,queue_size = 10)
    rospy.init_node('write_angles_node',anonymous = True)
    rate = rospy.Rate(10)

    # write 4 angles
    if len(sys.argv) == 5:
        s1 = int(sys.argv[1])
        s2 = int(sys.argv[2])
        s3 = int(sys.argv[3])
        s4 = int(sys.argv[4])
        pub.publish(s1,s2,s3,s4)

    else:
        raiseError()

    rate.sleep()

# main function

def main():
    rospy.init_node('follow_waypoints')

    sm = StateMachine(outcomes=['success'])

    with sm:
        StateMachine.add('GET_PATH', GetPath(),
                           transitions={'success':'FOLLOW_PATH'},
                           remapping={'waypoints':'waypoints'})
        StateMachine.add('FOLLOW_PATH', FollowPath(),
                           transitions={'success':'PATH_COMPLETE'},
                           remapping={'waypoints':'waypoints'})
        StateMachine.add('PATH_COMPLETE', PathComplete(),
                           transitions={'success':'GET_PATH'})

    outcome = sm.execute()

def main(limb_name, reset):
    """
    Parameters
    ----------
    limb : str
        Which limb to use. Choices are {'left', 'right'}
    reset : bool
        Whether to use previously saved picking and placing poses or
        to save new ones by using 0g mode and the OK cuff buttons.
    """
    # Initialise ros node
    rospy.init_node("pick_and_place", anonymous=False)


    b = Baxter(limb_name)
    place_pose = limb_pose(limb_name).tolist()
    print (place_pose)
    block = Blocks()
    rospy.sleep(4)
    pick_pose = block.pose
    rospy.loginfo('Block pose: %s' % pick_pose)
    #import ipdb; ipdb.set_trace()
    b.pick(pick_pose, controller=None)
    b.place(place_pose)

def main():
    rospy.init_node('smach_example_state_machine')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4'])
    sm.userdata.sm_counter = 0

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(),
                               transitions={'outcome1':'BAR',
                                            'outcome2':'outcome4'},
                               remapping={'foo_counter_in':'sm_counter',
                                          'foo_counter_out':'sm_counter'})
        smach.StateMachine.add('BAR', Bar(),
                               transitions={'outcome1':'FOO'},
                               remapping={'bar_counter_in':'sm_counter'})


    # Execute SMACH plan
    outcome = sm.execute()

def main():
    rospy.init_node('issue_com')
    pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10)
    test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10)
    sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen)
    #sub = rospy.Subscriber('/joint_states', JointState, listen)

    pc = PositionCommand()
    pc.mode = JOINT_SPACE
    #pc.arm = PositionCommand.LEFT_ARM
    pc.arm = 1#PositionCommand.RIGHT_ARM
    pc.data = np.zeros(7)

    r = rospy.Rate(1)
    #while not rospy.is_shutdown():
    #    pub.publish(pc)
    #    r.sleep()
    #    print 'published!'
    r.sleep()
    test_pub.publish(Empty())
    pub.publish(pc)

def __init__(self):
        self.node_name = "face_recog_fisher"
        rospy.init_node(self.node_name)

        rospy.on_shutdown(self.cleanup)
        self.bridge = CvBridge()
        self.face_names = StringArray()
        self.all_names = StringArray()

        self.size = 4
        face_haar = 'haarcascade_frontalface_default.xml'
        self.haar_cascade = cv2.CascadeClassifier(face_haar)
        self.face_dir = 'face_data_fisher'
        self.model = cv2.createFisherFaceRecognizer()
        # self.model = cv2.createEigenFaceRecognizer()

        (self.im_width, self.im_height) = (112, 92)        

        rospy.loginfo("Loading data...")
        # self.fisher_train_data()
        self.load_trained_data()
        rospy.sleep(3)        

        # self.img_sub = rospy.Subscriber("/asus/rgb/image_raw", Image, self.img_callback)
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback)

        # self.img_pub = rospy.Publisher('face_img', Image, queue_size=10)
        self.name_pub = rospy.Publisher('face_names', StringArray, queue_size=10)
        self.all_names_pub = rospy.Publisher('all_face_names', StringArray, queue_size=10)
        rospy.loginfo("Detecting faces...")

def __init__(self):
        self.node_name = "train_faces_eigen"
        rospy.init_node(self.node_name)

        rospy.on_shutdown(self.cleanup)
        self.bridge = CvBridge()

        self.size = 4
        face_haar = 'haarcascade_frontalface_default.xml'
        self.haar_cascade = cv2.CascadeClassifier(face_haar)
        self.face_dir = 'face_data_eigen'
        self.face_name = sys.argv[1]
        self.path = os.path.join(self.face_dir, self.face_name)
        # self.model = cv2.createFisherFaceRecognizer()
        self.model = cv2.createEigenFaceRecognizer()

        self.cp_rate = 5

        if not os.path.isdir(self.path):
            os.mkdir(self.path)

        self.count = 0    

        self.train_img_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback)
        # self.train_img_pub = rospy.Publisher('train_face', Image, queue_size=10)
        rospy.loginfo("Capturing data...")

def __init__(self):
        self.node_name = "hand_gestures"
        rospy.init_node(self.node_name)

        rospy.on_shutdown(self.cleanup)

        # self.cv_window_name = self.node_name
        # cv2.namedWindow("Depth Image", 1)
        # cv2.moveWindow("Depth Image", 20, 350)

        self.bridge = CvBridge()
        self.numFingers = RecognizeNumFingers() 

        self.depth_sub = rospy.Subscriber("/asus/depth/image_raw", Image, self.depth_callback)
        self.num_pub = rospy.Publisher('num_fingers', Int32, queue_size=10, latch=True)       
        # self.img_pub = rospy.Publisher('hand_img', Image, queue_size=10)
        rospy.loginfo("Waiting for image topics...")

def __init__(self):
        # rospy.init_node('nav_test', anonymous=False)

        #what to do if shut down (e.g. ctrl + C or failure)
        rospy.on_shutdown(self._shutdown)

        #tell the action client that we want to spin a thread by default
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        rospy.loginfo("waiting for the action server to come up...")
        #allow up to 5 seconds for the action server to come up
        self.move_base.wait_for_server(rospy.Duration(5))

        #we'll send a goal to the robot to tell it to move to a pose that's near the docking station
        self.goal = MoveBaseGoal()
        self.goal.target_pose.header.frame_id = 'odom'
        self.goal.target_pose.header.stamp = rospy.Time.now()

def __init__(self):
        self.node_name = "face_recog_eigen"
        rospy.init_node(self.node_name)

        rospy.on_shutdown(self.cleanup)
        self.bridge = CvBridge()
        self.face_names = StringArray()

        self.size = 4
        face_haar = 'haarcascade_frontalface_default.xml'
        self.haar_cascade = cv2.CascadeClassifier(face_haar)
        self.face_dir = 'face_data_eigen'
        # self.model = cv2.createFisherFaceRecognizer()
        self.model = cv2.createEigenFaceRecognizer()

        (self.im_width, self.im_height) = (112, 92)        

        rospy.loginfo("Loading data...")
        # self.fisher_train_data()
        self.load_trained_data()
        rospy.sleep(3)        

        # self.img_sub = rospy.Subscriber("/asus/rgb/image_raw", Image, self.img_callback)
        self.img_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback)

        # self.img_pub = rospy.Publisher('face_img', Image, queue_size=10)
        self.name_pub = rospy.Publisher('face_names', StringArray, queue_size=10)
        self.all_names_pub = rospy.Publisher('all_face_names', StringArray, queue_size=10)
        rospy.loginfo("Detecting faces...")

def main():
    rospy.init_node('issue_com')
    pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10)
    test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10)
    sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen)
    #sub = rospy.Subscriber('/joint_states', JointState, listen)

    pc = PositionCommand()
    pc.mode = JOINT_SPACE
    #pc.arm = PositionCommand.LEFT_ARM
    pc.arm = 1#PositionCommand.RIGHT_ARM
    pc.data = np.zeros(7)

    r = rospy.Rate(1)
    #while not rospy.is_shutdown():
    #    pub.publish(pc)
    #    r.sleep()
    #    print 'published!'
    r.sleep()
    test_pub.publish(Empty())
    pub.publish(pc)