ros_demo系列——smach

ros_demo系列——smach

此文章用来记录在学习roswiki上smach剩下的坑，后续有机会解决并修改。另外smach是一个功能强大且可扩展的基于python的分层状态机库，其不依赖ros，但是executive_smach提供了很好的ros支持，包括actionlib与Smach viewer 的整合，为机器人任务级的决策的提供了一个比较好的开发工具。

参考资料：

• http://wiki.ros.org/smach/Tutorials
• https://www.ncnynl.com/archives/201807/2506.html

#提供一个关于movebase的示例程序

import rospy 
from smach import StateMachine  
from smach_ros import SimpleActionState  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

waypoints =[    
    ['one', (6.575, 3.962), (0.0,0.0,6.575, 3.962)],  
    ['two', (3.190, 3.132), (0.000, 0.000, 0.921, -0.389)],
    ['three', (1.692, 1.433), (0.000, 0.000, -0.536, 0.844)],
    ['four', (3.153, -4.076), (0.000, 0.000, 0.121, 0.993)]
]

if __name__ == '__main__':
    rospy.init_node('patrol')
    patrol = StateMachine(['succeeded','aborted','preempted'])
    with patrol:
        for i,w in enumerate(waypoints):
            goal_pose = MoveBaseGoal()
            goal_pose.target_pose.header.frame_id = 'map'
            goal_pose.target_pose.pose.position.x = w[1][0]
            goal_pose.target_pose.pose.position.y = w[1][1]
            goal_pose.target_pose.pose.position.z = 0.0
            goal_pose.target_pose.pose.orientation.x = w[2][0]
            goal_pose.target_pose.pose.orientation.y = w[2][1]
            goal_pose.target_pose.pose.orientation.z = w[2][2]
            goal_pose.target_pose.pose.orientation.w = w[2][3]
            
            StateMachine.add(
            	w[0],
                SimpleActionState('move_base', MoveBaseAction, goal=goal_pose),
                transitions={'succeeded':waypoints[(i + 1) % len(waypoints)][0]}
            )

    patrol.execute()

暂时的坑：

• Smach viewer的配置环境问题
• 后续再更新一些更实用的demo