写在前面:零、(可能的)弃坑说明
本篇内容为笔者在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
<?xml version="1.0"?>
<launch>
<!--先启动 gazebo 并加载 模型关节消息 -->
<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上添加如下内容
<!-- 从yaml文件加载联合控制器的参数 -->
<rosparam file="$(find carpack_control)/config/controller_racecar.yaml" command="load"/>
<!-- 加载控制器 spawner -->
<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处添加一行代码:
<!--运行joint_state_publisher节点,向tf发布机器人关节状态-->
<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("<KeyPress>", keydown)
frame.bind("<KeyRelease>", 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里,
<!-- 启动按键控制器 用户编写的Python文件 -->
<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
<?xml version="1.0"?>
<launch>
<!--先启动 gazebo 并加载 模型关节消息 -->
<include file="$(find racecar_description)/launch/racecar.launch" />
<!-- 从yaml文件加载联合控制器的参数 -->
<rosparam file="$(find racecar_description)/config/controller_racecar.yaml" command="load"/>
<!-- 加载控制器 spawner -->
<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"/>
<!-- 启动按键控制器 用户编写的Python文件 -->
<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
的<size>
标签中
参考smartcar的model.sdf
文件,在参考链接上修改了两点:
1.这里把<normal>
注释了
2.把上头的<plane>
标签改成<box>
标签
记得上下有两处要修改
- 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
七、效果展示
文章来源:https://www.toymoban.com/news/detail-450953.html
看到这了,如果觉得有用的话点个赞吧,这是笨人的创作动力捏🥰🥰🥰文章来源地址https://www.toymoban.com/news/detail-450953.html
到了这里,关于ROS仿真gazebo小车寻迹PID【第一期】的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!