小车yolo机械臂（一）ros下gazebo搭建小车（可键盘控制）安装摄像头仿真 加载yolo检测识别标记物体
ros下gazebo搭建小车（可键盘控制）安装摄像头仿真 加载yolo检测识别标记物体
- 目录总览
- 一，项目代码下载
- 二，ros下实现darknet_ros(YOLO V3)检测
-
- 2.1 编译整个项目
- 2.2 下载模型
- 2.3运行代码
- 三，在gazebo中搭建小车及摄像头
-
- 3.1 最终结果预览
- 3.2 搭建小车的模型
-
- 3.2.1xacro声明及常量定义
- 3.2.2 惯性矩阵
- 3.2.3 xacro零部件定义
- 3.2.4 传动系统
- 3.2.5 添加Gazebo控制器插件
- 3.3，小车摄像头
- 3.4 小车和摄像头拼接
- 3.5 launch文件
-
- 3.5.1代码
- 3.5.2 运行launch
- 3.5.3 使用rqt查看摄像头内容
- 3.5.4 小车摄像头与yolo对接方法
- 四，加载YOLO，让小车通过darknet_ros识别检测标记物体
- 五，键盘控制小车的移动
目录总览
小车yolo机械臂（一）ros下gazebo搭建小车（可键盘控制）安装摄像头仿真 加载yolo检测识别标记物体
小车yolo机械臂（二）机械臂仿真 ros下从xacro模型文件搭建Moveit!+Gazebo仿真系统
小车yolo机械臂（三）ROS消息订阅监听 rospy.Subscriber 订阅监听yolo python实现订阅/darknet_ros/bounding_boxes topic
小车yolo机械臂（四）python ros 和darknet_ros 使用launch文件启动脚本
小车yolo机械臂（五）让小车在命令下运动起来 rostopic pub /cmd_vel geometry_msgs/Twist
小车yolo机械臂（六）ros gazebo 小车摄像头根据darknet_ros中yolo目标检测的信息进行自主运动
小车yolo机械臂（七）小车与机械臂融合，并控制机械臂运动 gazebo control
小车yolo机械臂（八）ros小车和机械臂gazebo仿真，机械臂根据darknet_ros中yolo检测结果来自动运动 python实现
一，项目代码下载
需要用到的代码我已经上传到github上面，有两个包，第一个是darknet_ros（用于yolo的目标检测），第二个是mrobot_gazebo（在ros下小车的仿真）。
下载地址：github项目
我们先看看整体的一个目录架构：
二，ros下实现darknet_ros(YOLO V3)检测
首先，我们先实现ROS下darknet_ros(YOLO V3)检测。
参考博客：ROS下实现darknet_ros(YOLO V3)检测
2.1 编译整个项目
在github讲darknet_ros下载在src目录后，在ROS工作空间目录下打开终端
然后执行命令：
catkin_make -DCMAKE_BUILD_TYPE=Release
此时会开始编译整个项目，编译完成后会检查{catkin_ws}/darknet_ros/darknet_ros/yolo_network_config/weights文件下有没有yolov2-tiny.weights和yolov3.weights两个模型文件，默认下载好的代码里面为了节省体积是不带这两个模型文件的。因此编译之后会自动开始下载模型文件，此时又是一段漫长的等待时间。
如果刚好你之前已经下载好了模型文件，那就好了，在开始编译之前就把模型文件拷贝到上述文件夹下，就不会再次下载了。
2.2 下载模型
建议提前下载模型文件：
下载链接：
https://pjreddie.com/media/files/yolov2.weights
https://pjreddie.com/media/files/yolov2-tiny.weights
https://pjreddie.com/media/files/yolov3.weights
https://pjreddie.com/media/files/yolov3-tiny.weights
其它方法的参考博客：
关于yolov3.weights文件下载地址的分享
2.3运行代码
- 图像话题发布
因为darknet_ros会直接订阅指定的图像话题名，然后对图像进行检测，绘制检测框，并发布相应的检测话题，因此首先需要找一个能够发布图像话题的ROS包，这里推荐使用ROS官方提供的usb_cam驱动包，可以直接将电脑自带摄像头或连接电脑的USB摄像头采集的图像发布为ROS图像话题。
下载摄像头驱动：
sudo apt-get install ros-kinetic-usb-cam
然后发布摄像头图像话题：
roslaunch usb_cam usb_cam-test.launch
如果顺利的话应该可以看到实际的图像显示界面。
2. 运行darknet_ros
然后执行darknet_ros进行检测，在运行检测之前需要更改一下配置文件，使得darknet_ros订阅的话题与usb_cam发布的图片话题对应。
打开darknet_ros/config/ros.yaml文件，找到：
将camera_reading中的topic修改为上图所示，即：
subscribers:
camera_reading:
topic: /usb_cam/image_raw
queue_size: 1
这里的topic: /usb_cam/image_raw是ros的电脑前置摄像头的topic，通过这个topic，yolo就可以拿到前置摄像头的图像。
回到darknet的工作空间根目录，执行：
source devel/setup.bash
roslaunch darknet_ros darknet_ros.launch
注意，在运行上面的命令时，roslaunch usb_cam usb_cam-test.launch这个命令也是在运行的哦。
这样就看到检测结果了，等后面把gazebo的小车和车上的摄像头做好了，再将 camera_reading: topic: /usb_cam/image_raw 改为仿真摄像头的topic。
三，在gazebo中搭建小车及摄像头
3.1 最终结果预览
接下来，我们就将在gazebo中搭建小车及摄像头。
先看看最终效果：
现在catkin_ws/src目录下打开终端，执行命令：
roslaunch mrobot_gazebo my_gazebo3.launch
可以看到：
3.2 搭建小车的模型
先搭建小车的模型：my_gazebo_robot_car.urdf.xacro
该模型是采用xcrao，是优化后的URDF模型，是一种精简化、可复用、模块化的描述形式。
3.2.1xacro声明及常量定义
xacro是URDF的升级版,模型文件的后缀名由.urdf变为.xacro,而
且在模型标签中需要加入xacro的声明:
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
定义所需要的颜色，小车身上的颜色：
<!-- Defining the colors used in this robot -->
<material name="Black">
<color rgba="0 0 0 1"/>
</material>
<material name="White">
<color rgba="1 1 1 1"/>
</material>
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
<material name="Red">
<color rgba="1 0 0 1"/>
</material>
常量定义
这里是对圆周率的pi进行定义：
<!--All units in m-kg-s-radians unit system -->
<xacro:property name="M_PI" value="3.1415926535897931" />
这一部分是对小车身上的零部件的常量定义
<!-- Main body length, width, height and mass
<xacro:property name="base_mass" value="0.5" /> -->
<xacro:property name="base_link_length_up" value="0.005"/>
<xacro:property name="base_link_radius" value="0.13"/>
<!-- Main body length, width, height and mass -->
<xacro:property name="base_link_mass" value="0.5" />
<xacro:property name="base_link_width" value="0.25"/>
<xacro:property name="base_link_length" value="0.28"/>
<xacro:property name="base_link_hight" value="0.005"/>
3.2.2 惯性矩阵
接下来就是惯性矩阵，这个知识点数学难度很高，建议知道结果就行，想要深入了解，有一篇博客和一套视频可以去观看：
这篇博客是在结果上的总结：机器人的惯性、视觉、碰撞特征计算与表示
这个是b站上的系统课程，看完后会对惯性矩阵有原理性的认识：理论力学
下面三个惯性矩阵的定义，其中包含球体惯性矩阵，立方体惯性矩阵，圆柱体惯性矩阵
<!-- Macro for inertia matrix -->
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
<xacro:macro name="box_inertial_matrix" params="m w h d">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(h*h+d*d)/12}" ixy = "0" ixz = "0"
iyy="${m*(w*w+d*d)/12}" iyz = "0"
izz="${m*(w*w+h*h)/12}" />
</inertial>
</xacro:macro>
对这些惯性矩阵的使用如下：
<sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}" />
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_height}" />
<box_inertial_matrix m="0.1" w="${base_link_width}" h="${base_link_hight}" d="${base_link_length}" />
3.2.3 xacro零部件定义
接下来是对小车每个零部件的宏定义（就不一一罗列了）
<!-- Macro for wheel joint -->
<xacro:macro name="wheel" params="lr translateY">
<!-- lr: left, right -->
<link name="wheel_${lr}_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0 " />
<geometry>
<cylinder length="${wheel_height}" radius="${wheel_radius}" />
</geometry>
<material name="Black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0 " />
<geometry>
<cylinder length="${wheel_height}" radius="${wheel_radius}" />
</geometry>
</collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_height}" />
</link>
<gazebo reference="wheel_${lr}_link">
<material>Gazebo/Black</material>
</gazebo>
<joint name="base_to_wheel_${lr}_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_${lr}_link"/>
<origin xyz="${motor_x} ${translateY * base_link_radius} 0" rpy="0 0 0" />
<axis xyz="0 1 0" rpy="0 0" />
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="wheel_${lr}_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_wheel_${lr}_joint" />
<actuator name="wheel_${lr}_joint_motor">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
注意，上面的代码中出现了：
<gazebo reference="wheel_${lr}_link">
<material>Gazebo/Black</material>
</gazebo>
这个是对小车颜色的再定义，因为在gazebo仿真中，小车的颜色只能通过该定义来完成，这是rviz和gazebo不兼容的一个地方，需要注意。
3.2.4 传动系统
传动系统(Transmission)可以将机器人的关节指令转换成执行器的控制信号。机器人每个需要运动的关节都需要配置相应的传动系统,可以在机器人的URDF模型文件中按照以下方法配置:
<!-- Transmission is important to link the joints and the controller -->
<transmission name="wheel_${lr}_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_wheel_${lr}_joint" />
<actuator name="wheel_${lr}_joint_motor">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
3.2.5 添加Gazebo控制器插件
Gazebo插件可以根据插件的作用范围应用到URDF模型的
(1)为
为
<gazebo>
<plugin name="unique_name" filename="plugin_name.so">
... plugin parameters ...
</plugin>
</gazebo>
与其他的
(2)为、
如果需要为、
<gazebo reference="your_link_name">
<plugin name=" unique_name " filename="plugin_name.so">
... plugin parameters ...
</plugin>
</gazebo>
至于Gazebo目前支持的插件种类,可以查看ROS默认安装路径下的/opt/ros/kinetic/lib文件夹,所有插件都是以libgazeboXXX.so的形式命名的。
Gazebo已经提供了一个用于差速控制的插件
libgazebo_ros_diff_drive.so,可以将其应用到现有的机器人模型上。在mrobot_gazebo/urdf/mrobot_body.urdf.xacro文件中添加如下插件声明（即小车的插件）:
<!-- controller -->
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>Debug</rosDebugLevel>
<publishWheelTF>true</publishWheelTF>
<robotNamespace>/</robotNamespace>
<publishTf>1</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>base_to_wheel_left_joint</leftJoint>
<rightJoint>base_to_wheel_right_joint</rightJoint>
<wheelSeparation>${base_link_radius*2}</wheelSeparation>
<wheelDiameter>${2*wheel_radius}</wheelDiameter>
<broadcastTF>1</broadcastTF>
<wheelTorque>30</wheelTorque>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic>
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
</gazebo>
在加载差速控制器插件的过程中,需要配置一系列参数,其中比较关键的参数如下。
寸,在计算差速参数时需要用到。
3.3，小车摄像头
小车模型搭建好了，下一个就是为小车添加一个摄像头，文件中的模型为：camera.xacro
为摄像头模型添加Gazebo插件
类似于小车模型中的差速控制器插件,传感器的Gazebo插件也需要在URDF模型中配置。
在camera.xacro的相关
颜色
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>
这个颜色相关的
与小车模型的配置相似,只需要设置颜色参数。
摄像头插件代码如下：
<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
重点是这个第二个设置摄像头插件的
3.4 小车和摄像头拼接
现在我们已经有了两个模型，一个是小车，一个是摄像头，现在讲两者组合在一起。
文件：my_gazebo_robot_All.urdf.xacro 就是将小车和摄像头组合在一起，代码如下：
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--car-->
<xacro:include filename="$(find mrobot_gazebo)/urdf/my_gazebo_robot_car.urdf.xacro" />
<xacro:include filename="$(find mrobot_gazebo)/urdf/camera.xacro" />
<xacro:property name="camera_offset_x" value="0" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.02" />
<!-- Body of mrobot, with plates, standoffs -->
<mrobot_body/>
<!-- Attach the Kinect -->
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
<parent link="plate_1_link"/>
<child link="camera_link"/>
</joint>
<xacro:usb_camera prefix="camera"/>
</robot>
3.5 launch文件
3.5.1代码
最后我们写一个launch文件，名为：my_gazebo3.launch
<launch>
<!-- 设置launch文件的参数 -->
<arg name="world_name" value="$(find mrobot_gazebo)/worlds/playground.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加载机器人模型描述参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/urdf/my_gazebo_robot_All.urdf.xacro'" /> \
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
</launch>
3.5.2 运行launch
在目录catkin_ws/src处启动终端，输入命令：
roslaunch mrobot_gazebo my_gazebo3.launch
gazebo仿真出现，可以看到下面的结果：
模型里有两个人和几个罐头，是我新添加的。
3.5.3 使用rqt查看摄像头内容
接着，我们使用rqt工具查看当前小车眼前的世界:
rqt_image_view
3.5.4 小车摄像头与yolo对接方法
有个地方需要注意，rqt工具中，我们可以看到
上图有个/camera/image_raw，这个就是小车身上摄像头的topic，在接下来的darknet_ros中，我们就要将darknet_ros/config/ros.yaml中的：
subscribers:
camera_reading:
topic: /usb_cam/image_raw
queue_size: 1
改为：
subscribers:
camera_reading:
topic: /camera/image_raw
queue_size: 1
四，加载YOLO，让小车通过darknet_ros识别检测标记物体
加载YOLO，让小车通过darknet_ros识别检测标记物体
在将darknet_ros/config/ros.yaml中的camera_reading:的topic改为 /camera/image_raw后，我们直接启动yolo：
roslaunch darknet_ros darknet_ros.launch
检测小车摄像头看到的物体是什么
检测结果：
五，键盘控制小车的移动
小车模型中已经加入了libgazebo_ros_diff_drive.so插件,可以使用差速控制器实现机器人运动。查看系统当前的话题列表
可以看到,Gazebo仿真中已经开始订阅cmd_vel话题了。接下来可以运行键盘控制节点,发布该话题的速度控制消息,小车就会在Gazebo中开始运动了。
roslaunch mrobot_teleop mrobot_teleop.launch
第一次执行一般会遇到这样的问题：
ROS_MASTER_URI=http://localhost:11311
ERROR: cannot launch node of type [mrobot_teleop/mrobot_teleop.py]: can’t locate node [mrobot_teleop.py] in package [mrobot_teleop]
No processes to monitor
shutting down processing monitor…
… shutting down processing monitor complete
问题原因
mrobot_teleop/mrobot_teleop.py文件没有执行权限
解决方法：
chmod +x mrobot_teleop.py
重新运行：
roslaunch mrobot_teleop mrobot_teleop.launch
