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

最新推荐文章于 2024-10-12 08:50:16 发布

ROS开发小白

最新推荐文章于 2024-10-12 08:50:16 发布

阅读量732

点赞数 1

分类专栏： ROS 机器人学 Python 文章标签： python

本文链接：https://blog.csdn.net/MuscleMonk/article/details/121649288

版权

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

摘要由CSDN通过智能技术生成

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

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

文章目录

- ROS_Python编程之案例代码核心解析（第三版）
- - 4. 自主巡逻案例

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