speech_smach.py 文件源码

def __init__(self):
        rospy.init_node("speech")
        rospy.on_shutdown(self.shutdown)

        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait up to 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))    

        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'base_link'
        nav_goal.target_pose.header.stamp = rospy.Time.now()
        q_angle = quaternion_from_euler(0, 0,0, axes='sxyz')
        q = Quaternion(*q_angle)
        nav_goal.target_pose.pose =Pose( Point(0.3,0,0),q)
        move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, 
                                                exec_timeout=rospy.Duration(60.0),
                                                server_wait_timeout=rospy.Duration(60.0))


        smach_top=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
        with smach_top:
            StateMachine.add("Wait_4_Statr", MonitorState("task_comming", xm_Task, self.start_cb),
                                                  transitions={'invalid':'NAV',
                                                               'valid':'Wait_4_Statr',
                                                               'preempted':'NAV'})

            StateMachine.add("NAV", move_base_state, transitions={"succeeded":"Wait_4_Stop","aborted":"NAV","preempted":"Wait_4_Stop"})
            StateMachine.add("Wait_4_Stop",MonitorState("task_comming", xm_Task, self.stop_cb),
                                                  transitions={'invalid':'',
                                                               'valid':'Wait_4_Stop',
                                                               'preempted':''})

            smach_top.execute()