SMACH 使用实例

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

该实例假设你已经理解了python、ROS、rospy以及actionlib相关知识。

创建功能包

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

$ 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

创建可执行程序

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

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

   1 #!/usr/bin/env python
   2 
   3 import roslib; roslib.load_manifest('smach_usecase')
   4 
   5 import rospy
   6 import smach
   7 
   8 def main():
   9     rospy.init_node('smach_usecase_executive')
  10 
  11     sm_root = smach.StateMachine(outcomes=[])
  12 
  13     with sm_root:
  14         pass
  15 
  16     outcome = sm_root.execute()
  17 
  18     rospy.spin()
  19 
  20 if __name__ == '__main__':
  21     main()

添加服务调用状态

添加两个状态到（当前为空）状态机"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)位置。

将SMACH查看器附加到内省

可以在执行时查看SMACH树的结构和状态。这是通过附加到给定的SMACH树的根进行内省服务器来完成。然后，smach_viewer可以在树上运行。

smach_viewer教程展示了如何完成这项工作。一旦你完成了添加内省服务器操作，你可以运行SMACH Viewer命令来查看:

$ rosrun smach_viewer smach_viewer.py

如果你在前一节中指定了你的状态为"RESET"和"SPAWN"，你应该看到这样的结构:

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

添加接口到Turtlesim形状行为

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

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

   1   <node pkg="turtle_actionlib" name="turtle_shape1" type="shape_server"/>
   2   <node pkg="turtle_actionlib" name="turtle_shape2" type="shape_server">
   3     <remap from="/turtle1/pose" to="/turtle2/pose"/>
   4     <remap from="/turtle1/cmd_vel" to="/turtle2/cmd_vel"/>
   5   </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。动作类型是，目标类型有两个参数(边、半径)。

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

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

并发绘画

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

首先构建一个并发，在 这里可以找到一个教程。在状态机中添加并发，然后将从根状态机发送形状目标的两个状态转换为并发。

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

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

请注意，这表明小多边形已经完成绘制，但大多边形仍然处于活动状态。

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

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

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

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

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

   1     ...
   2     child_termination_cb = lambda state_outcomes: True,
   3     ...

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

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

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

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

封装SMACH可执行文件到行为

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