ROS学习|SMACH状态机三(使用实例)

系列文章目录

ROS基础
ROS单线程与多线程
navigation基本导航
navigation基本导航
SMACH状态机一(安装与初探)
SMACH状态机三(使用实例)

---

写在前面

关于ROS实例官方出了两个文档，第一个实例代码和中文讲解都非常清楚，创客制造上也有详细的注释，所以我就不重复造轮子了，大家可以自己去看。第二个实例，官方代码不全，因此我就把官方文章搬运一下，顺便补全代码并加上一些注释。
想看官方文档和创客制造上的文章可以制造跳到最后，我在参考文献那里会放相关链接。
以下，Enjoy！

---

这个使用实例将探讨SMACH的可用性和学习曲线。这个用例从简单地使用SMACH API开始，并以一个与其他ROS系统接口的具体示例结束:一个在turtlesim中协调两只乌龟的程序。

1 创建功能包

首先，创建一个功能包，添加一组依赖:

$ roscreate-pkg smach_usecase rospy std_srvs smach turtlesim
$ roscd smach_usecase

创建一个launch文件来启动turtlesim环境。 我们将这个launch文件称为"turtle_nodes.launch":
切换行号显示

   1 <launch>
   2   <node pkg="turtlesim" name="turtlesim" type="turtlesim_node"/>
   3 </launch>

现在，你可以运行以下命令来启动turtlesim仿真环境:

$ roslaunch smach_usecase turtle_nodes.launch

！注意：这里需要在workspace中创建功能包。不懂的话复习下图

2 创建可执行程序

在"smach_usecase"包中创建"scripts"目录。在这个目录下，创建一个名为"executive.py"的python文件。这个降本将是示例的主要目标。

在这个脚本中，创建SMACH管理的主干部分。这涉及到，主要的imports、main()函数定义以及创建一个空的SMACH状态机。

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_usecase')

import rospy
import smach

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

    sm_root = smach.StateMachine(outcomes=[])

    with sm_root:
        pass

    outcome = sm_root.execute()

    rospy.spin()

if __name__ == '__main__':
    main()

3 添加服务调用状态

添加两个状态到（当前为空）状态机"sm_root"。第一个状态需要通过turtlesim调用ROS服务来重置模拟器。第二个状态需要在(0,0)点再生一个名为"turtle2"的乌龟。

这两个任务都执行服务调用,可以使用SMACHsmach.ServiceState实现。服务属性表示一个ROS服务调用的执行。它们提供了"成功(succeeded)"、"中止(aborted)"和"抢占(preempted)"的潜在结果。

• 第一个服务称为"reset"，并且服务类型为std_srvs.Empty。
• 第二个服务称为"spawn"，并且服务类型为turtlesim.srv.Spawn。它的请求类型：turtlesim.srv.SpawnRequest，需要四个参数(x,y, theta, name)

一旦你添加了这两个状态，你就应该能够启动turtlesim的launchfile，然后运行可执行程序。当你运行可执行程序时，你应该看到海龟被重置，然后一只新的海龟应该出现在(0,0)位置。
首先需要在文件前面import相关的库

import std_srvs.srv 
import turtlesim.srv
import smach_ros
from smach_ros import ServiceState

其次在main函数中添加如下代码即可。

sm_root = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

	with sm_root:
		smach.StateMachine.add('RESET',ServiceState('reset',std_srvs.srv.Empty),transitions={'succeeded':'SPAM'})
		smach.StateMachine.add('SPAM',ServiceState('spawn',turtlesim.srv.Spawn,request = turtlesim.srv.SpawnRequest(0,0,0,'turtle2')),transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'})
		
	
	# start  introspection server to use smach_viewr.py
	sis = smach_ros.IntrospectionServer('server_name',sm_root,'/SM_ROOT')
	sis.start()

运行完，terminal中应该出现下面的图片
注意，这里添加了IntrospectionServer是为了可视化任务状态图。
注意，可能有些人会找不到smach_viewer功能包。执行下面命令即可

sudo apt-get install ros-melodic-executive-smach-visualization
rosrun smach_viewer smach_viewer.py

4 可视化

执行完上面的命令 你应该会下面的结构
在上面的图像中，状态由椭圆表示，它们的结果由有向边表示。容器"sm_root"的结果显示为红色。这是一个很好的约定，可以将状态名大写，在小写的情况下写出结果，并使用下划线，在名称中有多个单词。

5 添加接口至turtlesim形状行为

接下来，你将添加一些任务序列。这些任务将涉及调用一些更多的服务，以及一些actionlib操作。在添加调用行为的代码之前，我们需要将行为服务器添加到launchfile中。

添加以下行到"turtle_nodes.launch"，这些将启动一对行为服务器来让两个乌龟画多边形:

  <node pkg="turtle_actionlib" name="turtle_shape1" type="shape_server"/>
  <node pkg="turtle_actionlib" name="turtle_shape2" type="shape_server">
    <remap from="/turtle1/pose" to="/turtle2/pose"/>
    <remap from="/turtle1/cmd_vel" to="/turtle2/cmd_vel"/>
  </node>

在添加行为状态之前，添加两个或更多服务状态。第一个需要移动"turtle1"到坐标(5.0,1.0)位置，并且第二个需要移动"turtle2"到坐标(9.0,5.0)位置。这些服务分别被"turtle1/teleport_absolute"和"turtle2/teleport_absolute"调用。它们使用服务类型turtlesim.srv.TeleportAbsolute，并且有(x, y, theta)三个参数作为请求类型。

接下来添加两个状态，将会发送目标到行为服务器"turtle_shape1"和"turtle_shape2"，这两个行为服务器我们在以上部分就已经添加完成。这个部分将会与smach.SimpleActionStates一起完成.

第一个状态应该是让turtle1画一个大的十一边形，半径为4.0，第二状态应该是让turtle2画一个小六边形，半径为0.5。动作类型是turtle_actionlib.msg.ShapeAction，目标类型有两个参数(边、半径)。

一旦你添加了上面描述的四个状态，你应该能够运行脚本并看到两个乌龟各自绘制一个多边形。
完整代码如下：

#!/usr/bin/env python

import roslib;roslib.load_manifest('smach_usecase')

import rospy  
import smach

import std_srvs.srv 
import turtlesim.srv
import smach_ros
from smach_ros import ServiceState
from smach_ros import SimpleActionState
import turtle_actionlib.msg
import turtlesim.msg




def main():
	rospy.init_node('smach_usecase_executive')
	
	sm_root = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

	with sm_root:
		# add state to call ROS service by turtlesim to reset sim
		smach.StateMachine.add('RESET',ServiceState('reset',std_srvs.srv.Empty),transitions={'succeeded':'SPAM'})
		# add state to spawn new turtle at 0,0 called turtle2
		smach.StateMachine.add('SPAM',ServiceState('spawn',turtlesim.srv.Spawn,request = turtlesim.srv.SpawnRequest(0,0,0,'turtle2')),
		transitions={'succeeded':'TELEPORT1'})
		
		smach.StateMachine.add('TELEPORT1', 
		ServiceState('turtle1/teleport_absolute',turtlesim.srv.TeleportAbsolute,
		 request = turtlesim.srv.TeleportAbsoluteRequest(5,1,0)),transitions={'succeeded':"TELEPORT2"})
		 
		smach.StateMachine.add('TELEPORT2', 
		ServiceState('turtle2/teleport_absolute',turtlesim.srv.TeleportAbsolute,
		 request = turtlesim.srv.TeleportAbsoluteRequest(9,5,0)),
		 transitions={'succeeded':"BIG"})

		smach.StateMachine.add('BIG', 
		SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
		 goal=turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4)),transitions={'succeeded':'SMALL'})

		smach.StateMachine.add('SMALL', 
		SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
		 goal=turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)),
		 transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'})

	
	
	# start  introspection server to use smach_viewr.py
	sis = smach_ros.IntrospectionServer('server_name',sm_root,'/SM_ROOT')
	sis.start()

	outcome = sm_root.execute()
	rospy.spin()
	 
	

if __name__ == '__main__':
	main()

如果你运行SMACH Viewer，并且使用了显示的命名约定，你应该可以看到如下图，turtle1正在绘制大多边形。

6 并发绘画

在接下来的步骤中，你将把这两个形状状态合并成一个并发。这将同时发送两个动作目标，并等待它们都终止。

首先构建一个并发。在状态机中添加并发，然后将从根状态机发送形状目标的两个状态转换为并发。

在执行代码时，您应该看到两只乌龟同时移动::

另外，如果你遵循相同的命名约定，那么SMACH查看器现在应该显示类似以下的结构:

请注意，这表明小多边形已经完成绘制，但大多边形仍然处于活动状态。
这里给出需要改动的代码：

smach.StateMachine.add('TELEPORT2', 
		ServiceState('turtle2/teleport_absolute',turtlesim.srv.TeleportAbsolute,
		 request = turtlesim.srv.TeleportAbsoluteRequest(9,5,0)),
		 transitions={'succeeded':"DRAW_SHAPES"})

		# Create DRAW SHAPES container
		sm_draw_shapes = smach.Concurrence(outcomes=['succeeded','preempted','aborted'],
		default_outcome= 'aborted',
		outcome_map={'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})

		smach.StateMachine.add('DRAW_SHAPES', sm_draw_shapes,transitions={'succeeded':'succeeded'})

		with sm_draw_shapes:
			smach.Concurrence.add('BIG', SimpleActionState('turtle_shape1',
			 turtle_actionlib.msg.ShapeAction,goal=turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4)))

			smach.Concurrence.add('SMALL', 
			 SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
			 goal=turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)))

7 当第一个海龟离得太近的时候，让第二个乌龟停下来。

在接下来的步骤中，你将会在turtle1离得太近的时候让turtle2停下来。这包括实现一个监视器模式。这种模式背后的思想是允许一个状态在某些条件不再满足时抢占(或停止)。

SMACH ROS库有一个监视器监控状态，它提供了将条件回调与话题和消息类型相关联的机制。每次状态接收到指定话题的消息时都会调用回调。如果回调返回True，那么它将继续阻塞，但是如果它返回False，它将终止，结果为’无效(invalid)’。可以找到更多的监控状态的 MonitorState教程页面。

就像一个状态可以与另一个状态并存。当条件不再成立时，唯一需要做的就是让它杀死相关状态，这是为了同时产生"子终止回调"。这个回调(如果指定的话)每次在一个SMACH并发终止时被调用，并确定其他状态是否应该被发送抢占信号。

当使用并发和监视器状态来实现此模式时，子终止回调可以简单地作为一个匿名函数，它总是返回True:

    ...
    child_termination_cb = lambda state_outcomes: True,
    ...

这样，终止的第一个状态将导致其他状态被抢占。如果遵循相同的命名约定，在对执行中的SMACH树进行了修改之后，SMACH查看器现在应该显示一个类似于以下的结构:

需要修改增加的代码为：

import turtlesim.msg
import actionlib_msgs
import threading
from smach_ros import MonitorState

turtle2pose = turtlesim.msg.Pose
def turtle2pose_cb(data):
	global turtle2pose
	turtle2pose = data
	
def monitor_callback(ud,turtle1pose):
	global turtle2pose
	dist = ((turtle1pose.x - turtle2pose.x)**2 + (turtle2pose.y-turtle1pose.y)**2)**(0.5)
	rospy.loginfo('distance between turtles: %s', dist)
	if(dist <2):
		return False
	else:
		return True

def child_term_cb(outcome_map):
	if outcome_map['MONITOR'] == 'invalid':
		return True
	elif outcome_map['DRAW'] == 'succeeeded':
		return True
	else:
		return False


...
# Create DRAW SHAPES container
		sm_draw_shapes = smach.Concurrence(outcomes=['succeeded','preempted','aborted'],
		default_outcome= 'aborted',
		outcome_map={'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})

		smach.StateMachine.add('DRAW_SHAPES', sm_draw_shapes,transitions={'succeeded':'succeeded'})

		with sm_draw_shapes:
			smach.Concurrence.add('BIG', SimpleActionState('turtle_shape1',
			 turtle_actionlib.msg.ShapeAction,goal=turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4)))

			 # create SMALL container with monitor
			sm_small_with_monitor = smach.Concurrence(outcomes=['succeeded','preempted','aborted'],
			 default_outcome='aborted',
			 child_termination_cb= child_term_cb,
			 outcome_map={'succeeded':{'DRAW':'succeeded'},
			 'preempted':{'DRAW':'preempted','MONITOR':'preempted'},'aborted':{'MONITOR':'invalid'}})

			smach.Concurrence.add('SMALL',sm_small_with_monitor)
			with sm_small_with_monitor:
				
				smach.Concurrence.add('DRAW',SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction, 
				goal=turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)))
				smach.Concurrence.add('MONITOR',MonitorState('turtle1/pose',turtlesim.msg.Pose,monitor_callback))

8 当第一个离开后，第二个乌龟重新开始

对这个执行器的行为的最后修改是，将一些简单的恢复行为添加到小多边形图中。在前面的步骤中，一旦turtle2停止画画，它就再也没有开始了。在此步骤中，你将需要添加另一个监视器，该监视器在海龟关闭时保持。

为了创建结果回环，你还需要将监视器和这个新的监视器一起放入状态机中。请参见下面的SMACH查看器中的图表，以更好地了解它的外观。
完整代码为：

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_usecase')
import rospy
import threading
import std_srvs.srv
import smach
import smach_ros
import turtlesim.srv
import turtlesim.msg
import turtle_actionlib.msg
import actionlib_msgs
from smach_ros import ServiceState
from smach_ros import SimpleActionState
from smach_ros import MonitorState
from smach_ros import ActionServerWrapper
turtle2pose = turtlesim.msg.Pose
def turtle2pose_cb(data):
    global turtle2pose
    turtle2pose = data

def monitor_callback(ud, turtle1pose):
    #rospy.loginfo('turtle1pose x: %f',turtle1pose.x )
    #rospy.loginfo('turtle2pose x: %f',turtle2pose.x )

    global turtle2pose
    dist = ((turtle1pose.x-turtle2pose.x)**2.0 + (turtle1pose.y-turtle2pose.y)**2.0 )**(1/2.0)
    rospy.loginfo('distance between turtles: %s',dist)
    if(dist < 2.0):
        return False
    else:    
        return True

def wait_for_clear_callback(ud,turtle1pose):
    global turtle2pose
    dist = ((turtle1pose.x-turtle2pose.x)**2.0 + (turtle1pose.y-turtle2pose.y)**2.0 )**(1/2.0)
    rospy.loginfo('distance between turtles: %s',dist)
    if(dist < 2.0):
        return True
    else:    
        return False

def child_term_cb(outcome_map):
    if outcome_map['MONITOR'] == 'invalid':
        return True
    elif outcome_map['DRAW'] == 'succeeded':
        return True
    else:
        return False



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

    rospy.Subscriber("turtle2/pose", turtlesim.msg.Pose, turtle2pose_cb)

    sm_root = smach.StateMachine(
        outcomes=['succeeded','preempted','aborted'])
        #input_keys = ['my_awesome_goal'],
        #output_keys= ['some_awesome_result']) 
    
    with sm_root:
        #add state to call ROS service by turtlesim to reset sim
        smach.StateMachine.add('RESET',
            ServiceState('reset',
            std_srvs.srv.Empty),
            transitions={'succeeded':'SPAWN'})
        #add state to spawn new turtle at 0,0 called turtle2
        smach.StateMachine.add('SPAWN',
            ServiceState('spawn',
            turtlesim.srv.Spawn,
            request = turtlesim.srv.SpawnRequest(0,0,0,'turtle2')),
            transitions={'succeeded':'TELEPORT1'})

        smach.StateMachine.add('TELEPORT1',
            ServiceState('turtle1/teleport_absolute',
            turtlesim.srv.TeleportAbsolute,
            request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0)),
            transitions={'succeeded':'DRAW_SHAPES'})
        
        # Create DRAW SHAPES container
        sm_draw_shapes = smach.Concurrence( outcomes=['succeeded','preempted','aborted'],
            default_outcome = 'aborted',
            outcome_map={'succeeded': {'BIG':'succeeded' , 'SMALL':'succeeded'}})

        smach.StateMachine.add('DRAW_SHAPES', sm_draw_shapes,
            transitions={'succeeded': 'succeeded'})

        with sm_draw_shapes:

            smach.Concurrence.add('BIG',
                SimpleActionState('turtle_shape1',
                turtle_actionlib.msg.ShapeAction,
                goal=turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0) ))
            
            #Create SMALL container
            sm_small = smach.StateMachine( outcomes=['succeeded','preempted','aborted'])

            smach.Concurrence.add('SMALL', sm_small)

            with sm_small:

                smach.StateMachine.add('TELEPORT2',
                    ServiceState('turtle2/teleport_absolute',
                    turtlesim.srv.TeleportAbsolute,
                    request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0)),
                    transitions={'succeeded':'DRAW_WITH_MONITOR'})

                smach.StateMachine.add('WAIT_FOR_CLEAR',
                    MonitorState("turtle1/pose",
                    turtlesim.msg.Pose,
                    wait_for_clear_callback),
                    transitions={'invalid':'TELEPORT2',
                                  'valid':'WAIT_FOR_CLEAR'})

                #Create DRAW WITH MONITOR 
                sm_draw_with_monitor = smach.Concurrence( outcomes=['succeeded','preempted','aborted','interrupted'],
                    default_outcome = 'aborted',
                    child_termination_cb = child_term_cb,
                    outcome_map={'succeeded': {'DRAW': 'succeeded', 'MONITOR':'valid'},
                                 'preempted': {'DRAW': 'preempted', 'MONITOR':'preempted'},
                                 'interrupted' : {'MONITOR': 'invalid'} })

                smach.StateMachine.add('DRAW_WITH_MONITOR', sm_draw_with_monitor,
                    transitions={'interrupted':'WAIT_FOR_CLEAR'})

                with sm_draw_with_monitor:

                    smach.Concurrence.add('DRAW',
                        SimpleActionState('turtle_shape2',
                        turtle_actionlib.msg.ShapeAction,
                        goal=turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) ))

                    smach.Concurrence.add('MONITOR',
                        MonitorState("turtle1/pose",
                        turtlesim.msg.Pose,
                        monitor_callback))

        
# Commented out to run as actionlib server
     #start introspection server to use smach_viewer.py
    sis = smach_ros.IntrospectionServer('server_name', sm_root, '/SM_ROOT')
    sis.start()

    # Execute SMACH tree in a separate thread so that we can ctrl-c the script
    smach_thread = threading.Thread(target = sm_root.execute)
    smach_thread.start()
    
    #outcome = sm_root.execute()

    rospy.spin()

    sis.stop()

    sm_root.request_preempt()
'''
asw = ActionServerWrapper(
        'smach_usecase_server', actionlib_msgs.msg.GoalID,
        wrapped_container = sm_root,
        succeeded_outcomes = ['succeeded'],
        aborted_outcomes = ['aborted'],
        preempted_outcomes = ['preempted'])
aws.run_server()
rospy.spin()    
'''

if __name__=='__main__':
    main()

9 封装SMACH可执行文件到行为

移除运行调用
添加封装代码
创建python调用行为
运行

参考文献

ROS官方案例1
创客制造关于案例1的讲解
ROS官方案例2