ROS_Python编程 之 案例代码核心解析（第三版）

最新推荐文章于 2025-11-27 09:11:38 发布
原创 最新推荐文章于 2025-11-27 09:11:38 发布 · 868 阅读 · 2 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#python

本文详细解析ROS_Python中的自主巡逻案例，基于multi_point_patrol.py代码，重点介绍了如何使用ROS的StateMachine、Containers和States进行路径规划。通过示例展示了如何创建和配置StateMachine、Concurrence容器，以及不同状态的交互和结果判断策略。
部署运行你感兴趣的模型镜像

ROS_Python编程 之 案例代码核心解析（第三版）

时隔有一段时间咯，忙着学习课内知识还有ros重要框架、接口，代码方面稍有懈怠，请大家见谅哈。通过Handsfree mini机器人平台配套的中级教程，我对ros_python 自主巡逻案例 的知识做以下归纳：

文章目录

4. 自主巡逻案例

handsfree mini 机器人工控机自带的ros系统有一份代码，是 multi_point_patrol.py，代码如下：

#!/usr/bin/env python
import tf
import math
import smach
import rospy
import actionlib
import geometry_msgs
import move_base_msgs.msg
import tf.transformations


def gen_new_movebase_goal(frame_id,
                          target_x,
                          target_y,
                          next_target_x,
                          next_target_y):
    goal = move_base_msgs.msg.MoveBaseGoal()
    goal.target_pose.header.frame_id = frame_id
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = target_x
    goal.target_pose.pose.position.y = target_y

    # to make our robot face the next target point,
    # when we arrive the target point

    target_yaw = (math.atan2(next_target_y - target_y, next_target_x - target_x) % (math.pi*2))
    # 二维平面中的欧拉角只有偏航角yaw，翻滚角、俯仰角均为0
    target_quaternion = tf.transformations.quaternion_from_euler(0, 0, target_yaw) 
    goal.target_pose.pose.orientation.x = target_quaternion[0]
    goal.target_pose.pose.orientation.y = target_quaternion[1]
    goal.target_pose.pose.orientation.z = target_quaternion[2]
    goal.target_pose.pose.orientation.w = target_quaternion[3]
    return goal


class MoveToPoint(smach.State):
    def __init__(self, target_x, target_y, next_target_x, next_target_y):
        smach.State.__init__(self, outcomes=['next_point'])
        self.__target_x = target_x
        self.__target_y = target_y
        self.__next_target_x = next_target_x
        self.__next_target_y = next_target_y

    def execute(self, ud):
        goal = gen_new_movebase_goal('map',
                                     self.__target_x,
                                     self.__target_y,
                                     self.__next_target_x,
                                     self.__next_target_y)
        server_movebase.send_goal(goal)
        # todo: what should robot do when failed to finish the target point?
        is_server_availabel = server_movebase.wait_for_result()
        if is_server_availabel is False:
            return 'error_exit'
        else:
            return 'next_point'


if __name__ == '__main__':
    rospy.init_node('smach_serial_demo2',log_level=rospy.DEBUG)
    server_movebase = actionlib.SimpleActionClient('/move_base', move_base_msgs.msg.MoveBaseAction)
    connect_state = server_movebase.wait_for_server()
    points = [[10.58,35.83],  # target point 1  only need x and y
              [8.88,12.18],  # target point 2
              [18.04,21.29],  # target point 3
              [18.75,36.58]]  # target point 4
    if connect_state is not True:
        # please to ensure weather your move_base server is opened?
        rospy.logerr('can not connect to move_base server')
        exit(-1)
    rospy.logdebug('move base connect successfully!')
    size_points = len(points)
    name_target_point = ""
    name_next_target_point = ""
    smach_serial_demo2 = smach.StateMachine(outcomes=['successful'])
    with smach_serial_demo2:
        # to point in points list into state machine
        for target_point in range(0, size_points, 1):
            next_target_point = (target_point+1) % size_points
            name_target_point = 'POINT%d' % target_point
            name_next_target_point = 'POINT%d' % next_target_point
            smach.StateMachine.add(name_target_point,
              MoveToPoint(target_x=points[target_point][0],
                          target_y=points[target_point][1],
                          next_target_x=points[next_target_point][0],
                          next_target_y=points[next_target_point][1]),
                          transitions={'next_point': name_next_target_point})
    smach_serial_demo2.execute()
    rospy.loginfo("HandsFree smach_serial_demo 2 finish!")

其中，精髓如下：

# 精髓1 
smach_serial_demo2 = smach.StateMachine(outcomes=['successful'])

# 解析1 因地制宜地应用了smach状态机
# 在收到一个任务时，第一步也是最重要的一步，即进行准确完整的需求分析：在自主巡逻案例中，需求即让机器人在四个固定目标点之间按照固定顺序进行循环运动：1->2->3->0->1->2->……，在做完需求分析之后，不可避免要进行方案分析：在自主巡逻案例中，机器人需要执行复杂的计划，其中包含多种可以被明确描述的状态和状态转换，这不禁让人联想到 smach状态机

# 解析2 smach状态机知识提点及较复杂的smach状态机案例剖析

【 以下部分内容出自 ROS 重要框架、接口学习资源小汇总 中的 ROS进阶——SMACH状态机 (注：若侵权，请告知删除) 】

smach的使用：

Containers（容器）：包含状态

  • StateMachine容器
  • Concurrence容器
  • Sequence容器
  • Iterator容器
  • 用actionlib封装容器

states（状态）：基本操作单元

  • state通用状态
  • CBState
  • SimpleActionState(ROS)
  • ServiceState(ROS)
  • MonitorState(ROS)

容器：

StateMachine：

from smach import State,StateMachine

# define state Foo
# 继承通用状态，构建Foo状态
class Foo(State):
 def __init__(self):
     State.__init__(self, 
                    outcomes=['outcome1','outcome2'],
                    input_keys=['foo_counter_in'],
                    output_keys=['foo_counter_out'])
     self.counter = 0

 # 响应调用的函数
 def execute(self, userdata):
     rospy.loginfo('Executing state FOO')
     if self.counter < 3:
         self.counter += 1
         return 'outcome1'
     else:
         return 'outcome2'

# define state Bar
class Bar(State):
 def __init__(self):
     State.__init__(self, outcomes=['outcome1'])

 def execute(self, userdata):
     rospy.loginfo('Executing state BAR')
     return 'outcome1'

class main():
     rospy.init_node('smach_example_state_machine')

 # Create a SMACH state machine
 sm = StateMachine(outcomes=[])

 # Open the container
 with sm:
     # Add states to the container
     StateMachine.add('FOO', 
                       Foo(), 
                       transitions={'outcome1':'BAR', 
                                    'outcome2':'outcome4'},
                       remapping={'foo_counter_in':'sm_counter', 
                                  'foo_counter_out':'sm_counter'})
     StateMachine.add('BAR', 
                      Bar(), 
                      transitions={'outcome1':'FOO'})

     # 内部监视器
     intro_server = IntrospectionServer('sm_IntrospectionServer', sm, '/SM_ROOT')
     intro_server.start()

     # Execute the state machine
     sm_outcome = sm_vison_work.execute()

     rospy.spin()

     intro_server.stop()

说明：

  1. 初始化：
__init__(self, outcomes, input_keys=[], output_keys=[])
  • outcomes，转移
  • input_keys，输入值
  • output_keys，输出值
  1. 添加状态：
add(label, state, transitions=None, remapping=None)
  • label，状态名字
  • state，状态
  • transitions，状态机状态跳转
  • remapping，状态机的输入输出映射到整体空间中，用于数据传递，input_keys为输入，output_keys为输出

Concurrence：并行状态机

import roslib; roslib.load_manifest('smach_tutorials')
import rospy
import smach
import smach_ros

# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'

# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        return 'outcome1'

# define state Bas
class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome3'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAS')
        return 'outcome3'

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

    # Create the top level SMACH state machine
    sm_top = smach.StateMachine(outcomes=['outcome6'])

    # Open the container
    with sm_top:
        smach.StateMachine.add('BAS', Bas(),
                               transitions={'outcome3':'CON'})

        # Create the sub SMACH state machine
        sm_con = smach.Concurrence(outcomes=['outcome4','outcome5'],
                                   default_outcome='outcome4',
                                   outcome_map={'outcome5':
                                       { 'FOO':'outcome2',
                                         'BAR':'outcome1'}})

        # Open the container
        with sm_con:
            # Add states to the container
            smach.Concurrence.add('FOO', Foo())
            smach.Concurrence.add('BAR', Bar())

        smach.StateMachine.add('CON', sm_con,
                               transitions={'outcome4':'CON',
                                            'outcome5':'outcome6'})

    # Execute SMACH plan
    outcome = sm_top.execute()

if __name__ == '__main__':
    main()

函数说明：

  1. 初始化
__init__(self, outcomes, default_outcome, input_keys=[], output_keys=[], outcome_map={}, outcome_cb=None, child_termination_cb=None)

  • outcome_map，指定基于其子结果确定并发结果的策略，即可根据多个子结果的组合输出指定输出，若映射没有满足，则返回default_outcome

  • outcome_cb，called once when the last child state terminates. This callback returns the outcome of the concurrence state machine

  • child_termination_cb，called every time one of the child states terminates. In the callback function you can decide if the state machine should keep running (return False), or if it should preempt all remaining running states (return True)

  1. 添加状态
add(label, state, remapping={})
  • 和StateMachine类似，但少了transitions，这部分通过outcome_map实现

具体流程说明：（注意：区分状态和状态机；状态的outcomes是转移，状态机的outcomes是输出！！！）

  1. sm_top = smach.StateMachine(outcomes=[‘outcome6’])

37

  1. smach.StateMachine.add(‘BAS’, Bas(),transitions={‘outcome3’:‘CON’})

38

  1. sm_con = smach.Concurrence( outcomes=[‘outcome4’,‘outcome5’],
    default_outcome=‘outcome4’,
    outcome_map={‘outcome5’:
    { ‘FOO’:‘outcome2’, ‘BAR’:‘outcome1’}} )

39

  1. smach.Concurrence.add(‘FOO’, Foo())

  2. smach.Concurrence.add(‘BAR’, Bar())

40

  1. smach.StateMachine.add( ‘CON’, sm_con,
    transitions={‘outcome4’:‘CON’,
    ‘outcome5’:‘outcome6’} )

41

# 解析3 自主巡逻案例中的状态机
    with smach_serial_demo2:
        # to point in points list into state machine
        for target_point in range(0, size_points, 1):
            next_target_point = (target_point+1) % size_points
            name_target_point = 'POINT%d' % target_point
            name_next_target_point = 'POINT%d' % next_target_point
            smach.StateMachine.add(name_target_point,
                   MoveToPoint(target_x=points[target_point][0],
                               target_y=points[target_point][1],
                               next_target_x=points[next_target_point][0],
                               next_target_y=points[next_target_point][1]),
                               transitions={'next_point': name_next_target_point})

class MoveToPoint(smach.State):
    def __init__(self, target_x, target_y, next_target_x, next_target_y):
        smach.State.__init__(self, outcomes=['next_point'])
        self.__target_x = target_x
        self.__target_y = target_y
        self.__next_target_x = next_target_x
        self.__next_target_y = next_target_y

# 解析3.1 
smach.State.__init__(self, outcomes=['next_point'])
# 在 for target_point in range(0, size_points, 1): 循环中，每调用一次MoveToPoint()，都会为 状态机smach_serial_demo2 的新状态定义 转移next_point

# 解析3.2
for target_point in range(0, size_points, 1):
    next_target_point = (target_point+1) % size_points
    name_target_point = 'POINT%d' % target_point
    name_next_target_point = 'POINT%d' % next_target_point
# target_point: 			0 -> 1 -> 2 -> 3
# next_target_point: 		1 -> 2 -> 3 -> 0
# name_target_point: 		'POINT0' -> 'POINT1' -> 'POINT2' -> 'POINT3'
# name_next_target_point:   'POINT1' -> 'POINT2' -> 'POINT3' -> 'POINT0'

因此，此案例中的自主巡逻路径如下：

42

# 解析3.3
smach.StateMachine.add(name_target_point,
                       MoveToPoint(target_x=points[target_point][0],
                                   target_y=points[target_point][1],
                                   next_target_x=points[next_target_point][0],
                                   next_target_y=points[next_target_point][1]),
                      transitions={'next_point': name_next_target_point})

因此，此案例中构成的状态机如下：

43

您可能感兴趣的与本文相关的镜像

Python3.8

Python3.8

Conda
Python

Python 是一种高级、解释型、通用的编程语言，以其简洁易读的语法而闻名，适用于广泛的应用，包括Web开发、数据分析、人工智能和自动化脚本

ROS知识：用python开发应用
gongdiwudu的专栏
04-22 1万+
在ROS工程，不仅需要熟悉C++；而且需要python，python存在比C++还要好的优势，就是可以和人工智能，图像处理相结合。因此，本文专门举了一个python开发项目，以强调其重要流程。
ROS_Python编程 之 案例代码核心解析（第一版）
MuscleMonk的博客
11-22 2831
ROS_Python编程 之 案例代码核心解析 通过Handsfree mini机器人平台配套的中级教程，我对ros_python编程实现 传感器数据读取、运动控制、自主导航 的知识做以下归纳： 文章目录ROS_Python编程 之 案例代码核心解析1. 传感器数据读取1.1 获取机器人底层硬件基本信息并打印1.2 获取IMU数据，并转化成欧拉角打印1.3 获取激光雷达数据并打印1.4 获取摄像头数据，并用opencv_python库显示2. 控制机器人移动2.1 控制机器人直线运动2.2 控制机器人转向运
ROS_by_example_groovy___volume_1(用python写的ROS教程)
03-09
利用Python编写的ROS_by_example系列教程，ROS_by_example_groovy___volume_1 英文资料，
ROS_Python编程 之 案例代码核心解析（第四版）
MuscleMonk的博客
12-02 1859
ROS_Python编程 之 案例代码核心解析（第四版） 通过Handsfree mini机器人平台配套的中级教程，我对ros_python 进阶编程 的知识做以下归纳： 文章目录ROS_Python编程 之 案例代码核心解析（第四版）5. 进阶编程5.1 get_angular_odom.py5.2 nav_obstacle.py5.3 nav_square.py 5. 进阶编程 handsfree mini 机器人工控机自带的ros系统一共有三份代码，分别是 get_angular_odom.py，na
ROS2中用Python开发服务接口（一）
最新发布
weixin_45809780的博客
11-27 677
WeatherQuery.srv：将的服务定义放在文件中。# 请求消息（客户端发送给服务端的参数）string city_name # 城市名称（必填，如 "Beijing"）string country_code # 国家代码（可选，如 "CN"，用于区分同名城市）---# 响应消息（服务端返回给客户端的结果）bool success # 查询是否成功（true=成功，false=失败）string message # 状态提示（如 "查询成功" 或 "未找到城市"）
ROS:用Python写一个简单服务
weixin_41698305的博客
06-05 2140
一.编写服务数据： 在功能包的顶级目录中，创建srv文件夹，并在里面创建.srv文件（先成为a.srv） 在srv文件中，填入服务数据，如： int64 a int64 b --- int64 sum 其中,---上方是请求数据，下方是答应数据 二.修改CmakeList和Package.xml： CmakeList： catkin_package( ... ...
ROS 话题通信（Python实现）
m0_73640344的博客
05-30 2010
简而言之，对于只包含Python节点的ROS软件包，主要用于初次将该软件包注册到ROS环境中。对于Python节点的后续修改或新增，通常不需要再次执行，除非你也在处理C++节点或需要更新软件包配置。Shebang是Python脚本中的一个重要特性，特别是在ROS环境中。它确保了脚本可以被正确的解释器执行，而无需在运行脚本时指定解释器。对于Python 2和Python 3，可能需要使用不同的shebang行，这取决于你的ROS环境和Python版本。
ROS_Python编程 之 案例代码核心解析（第二版）
MuscleMonk的博客
11-25 1614
ROS_Python编程 之 案例代码核心解析（第二版） 通过Handsfree mini机器人平台配套的中级教程，我对ros_python编程实现 自主导航 的知识做以下归纳： 文章目录ROS_Python编程 之 案例代码核心解析（第二版）3. 导航发送目标点3.1 set_goal.py3.2 set_two_goal.py3.3 set_four_goals.py 3. 导航发送目标点 通过 ros 的 navigation 接口向 move_base 导航系统发送 goal，让机器人导航到目标点。
ROS2发展历程与Python编程实战指南
bother3000的博客
07-20 916
摘要：ROS2作为新一代机器人操作系统，通过DDS通信架构实现去中心化设计，解决了ROS1的单点故障和实时性等问题。本文详细介绍了ROS2的发展历程、Python开发环境配置、面向对象编程实践以及节点开发深度技巧。重点内容包括：基于Python的异步编程模型、类型注解强化、OOP封装模式、通信机制解析、参数动态管理等工程化实践。同时提供了SLAM导航机器人项目实战案例，涵盖系统架构、核心代码实现及性能优化方案展望ROS2与Python融合开发的未来趋势，强调异步化整合和AI模型部署优化将成为发展方向。
《ROS2入门：Python在ROS2开发中的优势》
bother3000的博客
07-20 813
Python在ROS2开发中展现出显著优势：开发效率高（代码量比C++少40%-60%）、语法简洁（支持快速原型验证）、集成AI/ML库能力强（无缝对接PyTorch/OpenCV等），并具备出色的跨平台兼容性。其动态类型系统和丰富社区资源（如官方Python API和开源功能包）极大提升了调试效率和功能复用性，特别适合机器人算法验证和数据处理场景。相比C++，Python在快速迭代和AI集成方面具有不可替代的优势，是ROS2开发的高效选择。
ROS高效入门第六章 -- 自定义ROS服务，编写python server+client样例
cy1641395022的博客
05-15 1387
ROS高效入门第六章 -- 自定义ROS服务，编写python server+client样例
python网络编程（第3版）配套代码_python网络编程（第3版）配套代码_
10-02
《Python网络编程（第3版）》原书配套代码
(转载)使用Python编写ROS程序
weixin_41698305的博客
03-25 9382
一，定义msg消息在 你的功能包--》【beginner_tutorials】,新建msg消息目录,新建Num.msg文件$ roscd beginner_tutorials$ mkdir msg$ cd msg$ touch Num.msg$ rosed beginner_tutorials Num.msg...
python入门教程到项目开发_ROS与Python入门教程-搭建开发环境(PyCharm)
weixin_29424607的博客
02-20 364
安装步骤版本选择选择Linux版本，我选的是最新的Community 2017.3免费版Wget方式下载，建立下载目录tools，下载并安装.$ mkdir ~/tools$ cd ~/tools$ wget https://download.jetbrains.com/python/pycharm....tar.gz或使用其他工具下载后，在上传到tools目录安装,在tools目录下$ cd ~...
ROS python教程-actionlib-编写简单的action客户端及服务器
David's Tweet
09-02 4354
适用环境 ROS中的服务service是一问一答的形式，你来查询了，我就返给你要的信息。 action也有服务的概念，但是它不一样的地方是：不是一问一答，而多了一个反馈，它会不断反馈项目进度。如navigation下的move_base package，你设定了目标点，反馈信息可能是机器人在规划路径上的即时位姿，直到机器人到达目标点，返回SUCCEEDED消息。 所以action lib适用于...
python机器人开发教程_ROS与Python入门教程-actionlib-开发简单的action服务端
weixin_39873741的博客
11-24 479
ROS与Python入门教程-actionlib-开发简单的action服务端说明使用simple_action_server库创建Python的Fibonacci(斐波那契数列)action服务端计算Fibonacci数列的顺序，得到计算后的数列介绍actionlib_tutorials/simple_action_servers/fibonacci_server.py代码#! /usr/bin...
2.1ROS通信机制-案例
weixin_43831098的博客
08-16 640
本篇记录ROS话题通信，服务通信，参数服务器三个通信机制的实例
【ROS教程】 Python 版：ROS运行 Hello Word
衍生星球的博客
04-26 1732
创建工作空间目录（即工程根目录，注意此时还不是ROS工作空间，只是一个目录）初始化工作空间（即把当前目录初始化为一个ROS工作空间）创建功能包（ROS基本单元，可以理解为一个功能模块，每个工程至少有一个功能包）编辑源文件（写代码实现需求）编辑编译配置文件（ROS1使用CMake编译系统，这里编辑CMakeList.txt文件）编译工程（使用CMake编译ROS工程，生成可执行文件，即ros节点，下文均称作节点）不只是Hello World，所有ROS工程都是这样的流程。
2024年Python最新ros、c++基于类的编程基础
2401_84585205的博客
05-01 1211
Python崛起并且风靡，因为优点多、应用领域广、被大牛们认可。学习 Python 门槛很低，但它的晋级路线很多，通过它你能进入机器学习、数据挖掘、大数据，CS等更加高级的领域。Python可以做网络应用，可以做科学计算，数据分析，可以做网络爬虫，可以做机器学习、自然语言处理、可以写游戏、可以做桌面应用…Python可以做的很多，你需要学好基础，再选择明确的方向。这里给大家分享一份全套的 Python 学习资料，给那些想学习 Python 的小伙伴们一点帮助！
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值