[Documentation] [TitleIndex] [WordIndex

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)"的潜在结果。

一旦你添加了这两个状态,你就应该能够启动turtlesim的launchfile,然后运行可执行程序。当你运行可执行程序时,你应该看到海龟被重置,然后一只新的海龟应该出现在(0,0)位置。

uc02_ts.png

将SMACH查看器附加到内省

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

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

$ rosrun smach_viewer smach_viewer.py

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

uc03.png

在上面的图像中,状态由椭圆表示,它们的结果由有向边表示。容器"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。动作类型是turtle_actionlib.msg.ShapeAction,目标类型有两个参数(边、半径)。

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

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

uc04.png

并发绘画

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

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

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

uc05_ts.png

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

uc05.png

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

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

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

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

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

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

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

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

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

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

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

封装SMACH可执行文件到行为


2020-09-12 12:32