ROS仿真gazebo小车寻迹PID【第一期】

写在前面：零、（可能的）弃坑说明

本篇内容为笔者在2022.5.11完成的，与某个比赛相关，所以当时没有发布。
而现在是2023.2.21，当时的一番雄心壮志现在已经熄了大半，此外随着时间推移，笔者需要花更多的时间在准备考研上，因此这可能是一期没有后续的文章

即便如此，这一篇也实现了一个挺有趣的功能啦：在gazebo上调用摄像头

不过笔者自己还是觉得这个项目挺好玩的，如果有时间一定会更新下一期的！

欢迎其他同学们提出问题或顺着这个思路接着做做~

也欢迎在评论区或者私信交流

分界线

---

本篇的目标是使用智能车官方的小车模板，用ROS仿真gazebo小车寻迹PID
键盘控制小车 --> slam建图 --> gmapping导航【第一条线】
∟–>添加摄像头，赛道线 --> 部署文件实现赛道线的识别 --> PID控制小车过弯【第二条线】
第一条路应该已经有挺多前辈走过了，很多内容只是搬运，写的并不好，主要是本人想实现第二条路，实践一下PID的知识什么的…

本篇完成内容

• 导入小车模型

---

• 键盘控制小车
• slam建图
• gmapping导航

---

• 添加摄像头，赛道线
• 部署文件实现赛道线的识别
• PID控制小车过弯

ROS小车在Gazebo中实现阿克曼转向车的仿真

本文后续大部分挪用他的
教程 Re:Zero ROS （五）—— 导入模型，关节控制器述

这个博主真的良心，窥镜而自视，弗如远甚
保姆级别的教程，后悔没有早点看到

其中，第一条路（不出意外的话）参考的就是上文，及古月学院的gazebo课程

相比之下，古月的更简单更具体并且适配了Ubuntu20 （要钱；而博主的讲得更全面，受众更广（虽然个人初体验是《七篇文章带你体会ROS从入门到入土》 ，不过全程跟下来，做一遍收获肯定很大

---

准备材料：

下载地址：古月居racecar

不同版本可能存在一些兼容性，不知道gitee库作者还有没有更新，不过现在看README.md应该能够解决大部分上述问题

racecar_description（小车三维模型）

功能包，放在工作空间的src目录下
作用：放置小车模型以及启动Gazebo

racetrack.world（赛道模型）

建议在racecar_description新建一个文件夹"worlds"，放到该文件夹下面
原因会在后面提到~

smartcar_plane （起点终点线模型）

需放在主文件夹（“~/”）下的.gazebo/models中
隐藏文件夹需要通过ctrl + h点出
可能没有，则需要自行创建

control_plugin.py（控制器）

目前还没进行到这一步，不需要.py
文件放在工程中，需要获取控制话题（速度，角度），输出为Gazebo中小车零件上的某几个joint的动作（前轮的转角，四轮的速度）。

---

一、先把功能包跑起来

Step01 添加模型文件到~/.gazebo/models

将smartcar_plane整个放入主页面下的.gazebo/models中，如果没有"models"文件夹，需要自行创建

Step02 修改launch文件【最新版本无需此步骤】

• 找到文件名为racecar.launch的文件【github上不同版本这个文件的位置不同，最新版本无需此步骤】，按照注释进行修改
  根据提示，修改第21行，把$(find racecar_gazebo)修改为$(find racecar_description)；如下图所示：

补充：可以通过

grep -r "关键词字段" 路径

查找该路径下所有文件中匹配的文本内容，找到racecar_description的位置（在package.xml里）
这里$(find XXX【文件夹名字】)即该文件夹对应的绝对路径

Step03 编译并运行

• catkin_make，完成后关掉这个终端（source在当前终端可能不生效）
• 为了一劳永逸，可以在~/.bashrc添加环境路径

echo "source ~/你的工作空间/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

• 运行launch文件

roslaunch bringup racecar_gazebo.launch

如果launch文件无法运行（无法识别bringup功能包），那么在工作空间路径下，重新手动source一次；即：

cd ~/你的工作空间
source devel/setup.bash

Step04 结果展示

打开一个有地图且有车的界面，任务完成！

下面进行第二步，这个终端不要关！

二、分别使用gazebo及rviz打开小车

事实上，原文launch文件夹下两个文件就分别是打开gazebo和打开rviz的，并且都有部分的注释，图省事可以跳过这一节，因为本节最后放出的代码除了修改了几个路径之外跟原.launch代码无区别

指路（开头说的好博主），大部分挪用他的
教程 Re:Zero ROS （五）—— 导入模型，关节控制器述

gazebo和rviz区别*

rviz通常是调整小车参数，具体验证运行效果常常是在gazebo完成的，或者不恰当地说，rviz常常只在最前期修改参数和后期运行效果不好时的调试中使用

“启动gazebo的代码很简单，就是导入一个ros官方写好的launch，该launch已经写好了启动软件的内容，并预留了部分配置参数。如果想查看该launch可以使用roscd直接跳转目录，然后使用vim打开查看。有关launch具体的语法、标签含义，请自行另外学习。”

《浅谈Ros中使用launch启动文件的方法（一）》

rviz导入小车模型（urdf的导入）

新开一个终端，终端键入rviz

• 将空的rviz文件另存为到某处（可以新建一个文件夹），然后在rviz中添加robotModel【点击rviz左下角“add”按钮】
  
  此时，RobotModel会自动订阅之前打开的launch中的某个topic，并显示模型

“要往rviz内加载模型，先需要有urdf模型数据。在launch内直接调用xacro文件解析器urdf模型，运行robot_state_publisher节点，向tf发布机器人关节状态。最后启动rviz。”

---

按照原博主的方法，在Ubuntu18上并不能正常打开rviz

⚠️⚠️⚠️

• 由于笔者的系统版本是Ubuntu18，有的指令有变化了
  
  所以在对应的launch文件进行修改

可能遇到的情况

• 如果你一开始的rviz内没有模型，只有一堆白色物块，那是因为左边的Displays -> Global Options -> Fixed Frame没有选择模型的坐标系。如果模型没有正确的建立坐标关系就会变为白色
  
  为此，就需要你：
  在Displays -> Global Options -> Fixed Frame选择（注意是选择而不是手动打，虽然有的时候手动打也有用）其他坐标系，此处的map应该为base_link
  ——“若没有选项？”
  ——“重启”

加载不全解决办法

如果这一步仍然有东西没有加载出来，可以回到第一步，改为使用racecar_gazebo_rviz.launch文件
官方有自己的rviz文件

三、利用Topic的发布接受控制小车前进后退转向

写在前面

这一部分总共需要三个终端，分别启动racecar.launch，motor_control.py和keyboard_control.py
而我下面的过程是拆解了racecar.launch，并把两个py合成到了launch文件里。

---

指路（开头说的好博主），大部分挪用他的

教程 Re:Zero ROS （五）—— 导入模型，关节控制器述

• 1.新建一个启动文件car_control.launch用于启动小车

car_control.launch

<launch>
    
    <include file="$(find carpack_control)/launch/car_gazebo.launch" />
launch>

• 2.“然后我们需要加载底层联合控制器controller_manager和参数。首先需要加载参数，因为参数比较多，我们写在另一个文件内，然后再把这个写满参数的文件include进launch文件内。联合控制器的参数可以直接复制以下内容。ros-wiki内有controller_manager节点配置参数的编写方法。”

智能车仿真 —— 2020室外光电创意组线上仿真赛
controller_manager

放到对应目录（规范）

controller_racecar.yaml

racecar:
  # Publish all joint states --公布所有--------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
  

  # Velocity Controllers ----速度控制器---------------------
  left_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_rear_axle
    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}
  right_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_rear_axle
    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}
  left_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_front_axle
    pid: {p: 0.5, i: 0.0, d: 0.0, i_clamp: 0.0}
  right_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_front_axle
    pid: {p: 0.5, i: 0.0, d: 0.0, i_clamp: 0.0}


  # Position Controllers ---位置控制器-----------------------
  left_steering_hinge_position_controller:
    joint: left_steering_joint
    type: effort_controllers/JointPositionController
    pid: {p: 10.0, i: 0.0, d: 0.5}
  right_steering_hinge_position_controller:
    joint: right_steering_joint
    type: effort_controllers/JointPositionController
    pid: {p: 10.0, i: 0.0, d: 0.5}

• 3.启动control_manager节点，在car_control.launch上添加如下内容

	    
	<rosparam file="$(find carpack_control)/config/controller_racecar.yaml" command="load"/>
	
	
	<node name="controller_manager" pkg="controller_manager" type="spawner" 
	      respawn="false" output="screen" ns="/racecar" 
	      args="left_rear_wheel_velocity_controller       right_rear_wheel_velocity_controller
	            left_front_wheel_velocity_controller      right_front_wheel_velocity_controller
	            left_steering_hinge_position_controller   right_steering_hinge_position_controller
	            joint_state_controller"/>

这个时候roslaunch该文件，新建一个终端，输入rostopic list应该能看到如下效果

话题名称应该是由刚才的.yaml确定的，car_control的几个话题应该与之对应

此时运行launch的终端，显示界面应如下图（除了那个报错

• 4.“这里又出现另一个问题，gazebo节点发布的/racecar/joint_states话题与/robot_state_publisher节点订阅的/joint_states话题没有关联起来。这里有两个解决方法，要么重映射/robot_state_publisher节点发布的话题，要么重映射gazebo节点发布的话题。”

（可以通过rosrun rqt_gui rqt_gui查看节点或TF坐标关系）
节点关系在Plugins->Inrtro->Node
TF关系在Plugins->Visual->TF trees

• “采取第二种方法的话，重映射gazebo节点发布的话题会很麻烦，并不是直接在gazebo节点或是controller_manager节点下重映射操作。使用还是推荐直接重映射/robot_state_publisher节点。”
  在racecar.launch处添加一行代码：

	    
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
	    <param name="publish_frequency" type="double" value="20.0" />
	    <remap from="/joint_states" to="/racecar/joint_states"/>
	node>

• 5.终于可以开始编写用户端代码了，可喜可贺！

直接新建一个功能包car_control，新建文件夹scripts(文件规范)

cd 工作空间/src
catkin_create_pkg car_control rospy roscpp rosmsg

附motor_control.py代码，功能：接收“/motor_control”的话题消息，收到后发布速度消息
motor_control.py

#!/usr/bin/env python
#-*-coding:utf-8-*-

# 加载ros的Python基础包
import rospy
# 加载topic话题 的 msg消息
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist

class main_class:
    # 初始化函数
    def __init__(self):
        # 创建node节点 —— 电机控制
        rospy.init_node('motor_control', anonymous=True)
        # 订阅topic话题 —— 电机pwm输出
        rospy.Subscriber("motor_output", Twist, self.callback)
        # 发布topic话题 —— 线速度输出
        self.pub_lrw = rospy.Publisher('/racecar/left_rear_wheel_velocity_controller/command', Float64, queue_size=10)
        self.pub_rrw = rospy.Publisher('/racecar/right_rear_wheel_velocity_controller/command', Float64, queue_size=10)
        self.pub_lfw = rospy.Publisher('/racecar/left_front_wheel_velocity_controller/command', Float64, queue_size=10)
        self.pub_rfw = rospy.Publisher('/racecar/right_front_wheel_velocity_controller/command', Float64, queue_size=10)
        # 发布topic话题 —— 角速度输出
        self.pub_lsh = rospy.Publisher('/racecar/left_steering_hinge_position_controller/command', Float64, queue_size=10)
        self.pub_rsh = rospy.Publisher('/racecar/right_steering_hinge_position_controller/command', Float64, queue_size=10)
        # 阻塞等待
        rospy.spin()
    # 回调函数
    def callback(self,data):
        # 创建 msg 消息, 注意:ros的float64是一个结构体
        angle = Float64()
        speed = Float64()
        # 提取 线速度 与 角速度
        speed.data = ((data.linear.x) * 8)
        angle.data = ((data.angular.z) * 1)
        # 向topic话题 发送 msg消息
        self.pub_lrw.publish(speed.data)
        self.pub_rrw.publish(speed.data)
        self.pub_lfw.publish(speed.data)
        self.pub_rfw.publish(speed.data)
        self.pub_lsh.publish(angle.data)
        self.pub_rsh.publish(angle.data)


if __name__ == '__main__':
    try:
        main_class()
    except rospy.ROSInterruptException:
        pass

“/motor_control”话题的发布可直接新建一个publish发布固定值，也可以稍微进行拓展，使用键盘控制
附键盘控制端代码
keyboard_control.py

#!/usr/bin/env python
#-*-coding:utf-8-*-

# Copyright (c) 2019, The Personal Robotics Lab, The MuSHR Team, The Contributors of MuSHR
# License: BSD 3-Clause. See LICENSE.md file in root directory.

import atexit
import os
import signal
from threading import Lock
from Tkinter import Frame, Label, Tk

import rospy
from geometry_msgs.msg import Twist

# 定义全局变量
UP = "w"
LEFT = "a"
DOWN = "s"
RIGHT = "d"
QUIT = "q"
# 状态
state = [False, False, False, False]
# python多线程语法，创建一个锁，防止多个程序同时调用打印功能
state_lock = Lock()
# 初始化赋值
state_pub = None
root = None
control = False

# 检测哪个按键被按下
def keyeq(e, c):
    return e.char == c or e.keysym == c

# 按键是否被松开
def keyup(e):
    global state
    global control
    # python语法
    with state_lock:
        if keyeq(e, UP):
            state[0] = False
        elif keyeq(e, LEFT):
            state[1] = False
        elif keyeq(e, DOWN):
            state[2] = False
        elif keyeq(e, RIGHT):
            state[3] = False
        control = sum(state) > 0

# 按键是否被按下？
def keydown(e):
    global state
    global control
    # python语法
    with state_lock:
        if keyeq(e, QUIT):
            shutdown()
        elif keyeq(e, UP):
            state[0] = True
            state[2] = False
        elif keyeq(e, LEFT):
            state[1] = True
            state[3] = False
        elif keyeq(e, DOWN):
            state[2] = True
            state[0] = False
        elif keyeq(e, RIGHT):
            state[3] = True
            state[1] = False
        control = sum(state) > 0

# Up -> linear.x = 1.0
# Down -> linear.x = -1.0
# Left ->  angular.z = 1.0
# Right -> angular.z = -1.0

# 订阅话题的回调函数
def publish_cb(_):
    with state_lock:
        if not control:
            return
        # 创建msg消息
        ack = Twist()
        # 根据按下的键赋值
        if state[0]:
            ack.linear.x = max_velocity
        elif state[2]:
            ack.linear.x = -max_velocity

        if state[1]:
            ack.angular.z = max_steering_angle
        elif state[3]:
            ack.angular.z = -max_steering_angle
        # 话题发布消息
        if state_pub is not None:
            state_pub.publish(ack)

def exit_func():
    # system函数可以将字符串转化成命令在服务器上运行；
    os.system("xset r on")

def shutdown():
    root.destroy()
    # destroy()只是终止mainloop并删除所有小部件
    rospy.signal_shutdown("shutdown")

# 主函数
def main():
    global state_pub
    global root
    # 声明全局变量
    global max_velocity
    global max_steering_angle
    # 获取变量，从参数服务器中中
    max_velocity = rospy.get_param("~speed", 2.0)
    max_steering_angle = rospy.get_param("~max_steering_angle", 0.34)
    # 不设立默认值了，确保有写
    key_publisher = "/motor_output"
    # 创建topic话题，发送按键信息
    state_pub = rospy.Publisher(
        key_publisher, Twist, queue_size=1
    )
    # 创建周期性调用函数“publish_cb”，频率是1/0.1=10Hz，
    rospy.Timer(rospy.Duration(0.1), publish_cb)
    # 注册函数。在程序结束时，先注册的后运行 
    atexit.register(exit_func)
    os.system("xset r off")
    # 创建窗口对象的背景色
    root = Tk()
    # 框架控件；在屏幕上显示一个矩形区域，多用来作为容器
    frame = Frame(root, width=100, height=100)
    frame.bind("", keydown)
    frame.bind("", keyup)
    frame.pack()
    frame.focus_set()
    # 窗口显示
    lab = Label(
        frame,
        height=10,
        width=30,
        text="Focus on this window\nand use the WASD keys\nto drive the car.\n\nPress Q to quit",
    )
    lab.pack()
    print("Press %c to quit" % QUIT)
    root.mainloop()

if __name__ == "__main__":
    rospy.init_node("key_control", disable_signals=True)
    # 安全操作
    signal.signal(signal.SIGINT, lambda s, f: shutdown())
    main()

然后将两个.py文件可以放在car_control.launch里，

  
    <node name="motor_control" pkg="car_control" type="motor_control.py"/>
    <node name="key_control" pkg="car_control" type="keyboard_control.py"/>

全部
car_control.launch

<launch>
    
    <include file="$(find racecar_description)/launch/racecar.launch" />
    
    
    <rosparam file="$(find racecar_description)/config/controller_racecar.yaml" command="load"/>
	
    
    <node name="controller_manager" pkg="controller_manager" type="spawner" 
	      respawn="false" output="screen" ns="/racecar" 
	      args="left_rear_wheel_velocity_controller       right_rear_wheel_velocity_controller
	            left_front_wheel_velocity_controller      right_front_wheel_velocity_controller
	            left_steering_hinge_position_controller   right_steering_hinge_position_controller
	            joint_state_controller"/>

    
    <node name="motor_control" pkg="car_control" type="motor_control.py"/>
    <node name="key_control" pkg="car_control" type="keyboard_control.py"/>


launch>

记得将两个py权限修改下，并且记得catkin_make(.launch文件不需要，.py需要)

• 6.roslaunch car_control.launch

可以玩耍了！
例程部分完成，接下来是创新部分

---

四、找到并接收摄像机话题

小车模型中自带摄像机。
根据urdf/sensor/camera.xacro中的内容，
确定图片大小为1280*720，topic频率为30Hz，话题名为/camera/image_raw

通过rostopic info /camera/image_raw可以看到话题的信息，为sensor_msgs/IMAGE类
借助python中的cv_bridge类将ROS话题类型转为cv2的numpy信息
show.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import cv2 as cv
import numpy as np
from cv_bridge import *
from sensor_msgs.msg import Image

class imager:
    def __init__(self):

        image_topic = '/camera/image_raw'
        self.cv_bridge = CvBridge()
        self.img_origin = np.zeros((1280, 720, 3), np.uint8)
        rospy.Subscriber(image_topic, Image, self.image_callback)


    def image_callback(self, msg):
        try:
            self.img_origin = self.cv_bridge.imgmsg_to_cv2(msg, 'bgr8')
        except CvBridgeError as err:
            rospy.logerr(err)


if __name__ == '__main__':
    # ROS节点初始化
    rospy.init_node('camera', anonymous=True)

    img = imager()
    while not rospy.is_shutdown():
        img_origin = img.img_origin
        cv.imshow("SHOW", img_origin)
        cv.waitKey(1)

添加show.py为可执行文件，不然可能搜索不到

cd racecar_description/scripts
chmod +x show.py

---

五、新建world，添加赛道元素

浅看了一眼，添加赛道元素的方法：

• 0.自己随便画一个图
• 1.在~/.gazebo/model中新建一个新的model，方法如下，或者参考racecar的

gazebo中使用自定义图片建立带纹理的地面模型方法

修改图片的大小：在model.sdf的标签中
参考smartcar的model.sdf文件，在参考链接上修改了两点：
1.这里把注释了
2.把上头的标签改成标签

记得上下有两处要修改

• 2.生成.world文件，参考链接

Gazebo创建围墙并生成.world文件

但是不能完全参考链接了，还要结合前面那个纹理的链接
大概描述下，打开两个终端，roscore，gazebo，然后点击Insert，如果第一步配置正确的话，会看到如下图片，点击导入进去，随便选个位置【主要是不会选】

• 3.点击 Edit->Building Editor，在地图周围建墙

（从这一步开始跟链接就一样了

保存后【此处保存为文件夹而不是.world】，点击 Edit-> Exit Building Editor

• 4.退出后，点击File -> Save World As 选择相应的路径，然后命名需要自己加上后缀.world

六、最后的修改：替换地图文件

• 修改racecar_description/launch/racecar中的world文件为上文保存的world文件

启动launch

roslaunch racecar_description car_control.launch

等上一个文件加载好后

启动摄像头显示文件

rosrun car_control show.py

七、效果展示

看到这了，如果觉得有用的话点个赞吧，这是笨人的创作动力捏

---