python类init_node()的实例源码

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)