ros小车固定路径导航并且遇到障碍停止

一.思路

        固定路径导航的思路以及源码来自于这位博主，我只是在他的代码基础上进行了移植。

ROS中编写自己全局路径规划插件实现固定路线规划（2）_ros中实现固定路径规划-CSDN博客

遇见障碍物停止则是在move_base与小车底盘节点中增加了一个节点来对不同的cmd_vel进行处理，再发布给地盘。

二.实现流程

1.首先根据博主的教程编写read_road以及save_road两个节点，第二个用来保存你的固定路径，第一个则是用来在导航运行时读取固定路径。

2.随后编写固定路径的全局规划器，并且在package.xml中对自己的全局路劲进行注册。随后运行命令查看一下是否修改成功。由于具体过程都在上面的链接中，我就不再赘述。需要注意的是要在cmaklist里面将源代码cpp文件添加进去

3.编写速度控制节点，如下。可以根据自己的需求进行更改

import rospy
import os
import subprocess
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class ObstacleAvoidance:
    def __init__(self):
        self.move_cmd = Twist()
        self.pub1 = rospy.Publisher('/cmd_vel1', Twist, queue_size=1)
        self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)

    def callback(self, msg):
        if min(msg.ranges)<1.5:
            self.move_cmd.linear.x = 0
            self.move_cmd.angular.z = 0
            print("停止前进，因为检测到障碍物在1.5米内")
        else:
            self.move_cmd.linear.x = 0.05
            self.move_cmd.angular.z = 0.05
            print("前进，因为没有检测到近距离的障碍物")
        self.pub1.publish(self.move_cmd)
        #print("发布速度命令")

class CmdVelCoordinator:
    def __init__(self):
        self.your_cmd = None
        self.move_base_cmd = None
        self.final_cmd = Twist()

        self.pub = rospy.Publisher('/cmd_vel2', Twist, queue_size=1)

        rospy.Subscriber('/cmd_vel1', Twist, self.your_cmd_callback)
        rospy.Subscriber('/cmd_vel', Twist, self.move_base_cmd_callback)

    def your_cmd_callback(self, msg):
        self.your_cmd = msg
        

    def move_base_cmd_callback(self, msg):
        self.move_base_cmd = msg
        

    def publish_cmd(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.your_cmd and self.your_cmd.linear.x == 0.0 and self.your_cmd.angular.z == 0.0:
                self.final_cmd = self.your_cmd
                print("发布停止速度")
                command = ["mplayer", ma3_file]
                subprocess.call(command)
            elif self.move_base_cmd:
                self.final_cmd = self.move_base_cmd
                print("x:%f",self.final_cmd.linear.x)
                print("z:%f",self.final_cmd.angular.z)
                print("发布movebase速度")

            self.pub.publish(self.final_cmd)
            rate.sleep()

if __name__ == '__main__':
    rospy.init_node('coordinator_avoidance_node')

    avoidance = ObstacleAvoidance()
    coordinator = CmdVelCoordinator()
    coordinator.publish_cmd()
        
    rospy.spin()

4.按照你写的脚本修改底盘订阅的cmd_vel，因为修改move_base发布的需要在源代码修改，比较麻烦，所以直接修改小车底盘订阅的速度。应该修改为以下你发布的cmd_vel2。

self.final_cmd = Twist()

self.pub = rospy.Publisher('/cmd_vel2', Twist, queue_size=1)

这样就能实现小车在使用固定路径导航能够碰见障碍物停下来而不是避开他的功能。

三.运行

在运行时，打开三个终端，一个终端运行导航节点，一个终端运行read_road节点，一个终端运行速度发布脚本。