ROS入门(八)——仿真机器人四（Gazebo+Rviz+雷达、摄像头、kinet仿真显示）

所用的学习链接：

【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程P278-288

【以上视频笔记见http://www.autolabor.com.cn/book/ROSTutorials/】

官方参考链接：http://gazebosim.org/tutorials?tut=ros_gzplugins

前文参考

ROS入门(五)——仿真机器人一（URDF+Rviz）

ROS入门(六)——仿真机器人二（Xacro+Rviz+Arbotix小车运动）

ROS入门(七)——仿真机器人三（Gazebo+Xacro）

一、介绍

1.目标

1.运动控制以及里程计信息显示
2.雷达信息仿真以及显示
3.摄像头信息仿真以及显示
4.kinect 信息仿真以及显示

2.ros_control

为了提高机器人系统在不同平台上的可移植性，ROS内置了一个解决方案ros_control。包含了各种接口以实现不同平台上的机器人系统兼容，提高程序的效率和灵活性。

二、实操流程

1.运动控制

（1）运动控制 t6_move.xacro文件

在前面章节中已经创建了一个机器人模型，接下来需要编写一个单独的 xacro 文件【t6_move.xacro】，为机器人模型添加传动装置以及控制器

<!-- 文件内容：实现两轮差速配置 -->
<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 传动实现:用于连接控制器与关节 -->
    <xacro:macro name="joint_trans" params="joint_name">
        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- 每一个驱动轮都需要配置传动装置 -->
    <xacro:joint_trans joint_name="left_wheel2base_link" />
    <xacro:joint_trans joint_name="right_wheel2base_link" />

    <!-- 控制器 -->
    <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>left_wheel2base_link</leftJoint> <!-- 左轮 -->
            <rightJoint>right_wheel2base_link</rightJoint> <!-- 右轮 -->
            <wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 车轮间距 -->
            <wheelDiameter>${wheel_radius * 2}</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>
</robot>

（2）xacro集成文件 t7_12346.xacro

新建 t7_12346.xacro

<!-- 组合惯性矩阵文件、小车、摄像头、雷达和运动控制 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="t1_head.xacro" />
    <xacro:include filename="t2_car.xacro" />
    <xacro:include filename="t3_camera.xacro" />
    <xacro:include filename="t4_laser.xacro" />
    <xacro:include filename="t6_move.xacro" />
</robot>

（3）launch文件启动gazebo

新建 t7_gazebo.launch

<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf_gazebo)/urdf/t7_12346.xacro" />
    <!-- 启动 gazebo -->

    <!-- 加载仿真环境 -->
    <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch" /> -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find urdf_gazebo)/worlds/box_house.world"></arg>
    </include>

    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>

• ctrl+shift+b编译
• 新终端1

roscore

• 新终端2

roslaunch urdf_gazebo t7_gazebo.launch

 （4）launch文件启动rviz

 新建 t7_rviz.launch

<launch>
    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

    <!-- 关节以及机器人状态发布节点 -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

</launch>

• 新终端3

roslaunch urdf_gazebo t7_rviz.launch

（5）小车运动

查看话题列表，会发现多了 /cmd_vel 然后可以发布 vmd_vel 消息控制。

•  方法一（推荐）

①如果没有安装teleop-twist-keyboard，则进行安装

# sudo apt-get install ros-<ros版本>-teleop-twist-keyboard
sudo apt-get install ros-noetic-teleop-twist-keyboard

 ②启动键盘方向控制

rosrun teleop_twist_keyboard teleop_twist_keyboard.py 

运行完就可以用键盘控制小车运动【注意要要让当前窗口为运行该代码的终端，小车才能运动】

 常用按键：

u ：左侧逆时针圆周           i ：前进          o ：右侧顺时针圆周
j ：逆时针原地运动           k ：停止          l ：顺时针原地运动
m ：左侧顺时针圆周           , ：后退          . ：右侧逆时针圆周

q/z : increase/decrease max speeds by 10%（速度提高/降低10%）
w/x : increase/decrease only linear speed by 10%（线速度提高/降低10%）
e/c : increase/decrease only angular speed by 10%（角速度提高/降低10%）

• 方法二

使用命令控制(或者可以编写单独的节点控制)，在终端运行代码后发现gazebo中的小车运动了

# 查看话题列表
# rostopic list

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

（6）Rviz信息展示

①左侧设置Global Options→Fixed Frame：odom

②左下角Add→RobotModel、Axes、TF、Odometry

③左侧设置Odometry→Topic：/odom，并取消勾选Covariance【不然页面会呈现透明黄色】

④左上角File→Save config as→7.19_demo01/urdf_gazrbo目录下新建config文件夹→保存为 t7_car.rviz

修改 t7_rviz.launch的环境导入，可以重启重新加载

# 原句子
# <node pkg="rviz" type="rviz" name="rviz" />
 
# 更新后
<node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find urdf_gazebo)/config/t7_car.rviz" />

控制小车在gazebo运动时，rviz将显示相关信息

2.雷达、摄像头信息仿真

（1）雷达传感器 t8_sensor_laser.xacro

 新建 t8_sensor_laser.xacro

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 配置雷达传感器信息 -->
    <gazebo reference="laser">
        <sensor type="ray" name="rplidar">
            <pose>0 0 0 0 0 0</pose>
            <visualize>true</visualize>
            <update_rate>5.5</update_rate>
            <ray>
                <scan>
                    <horizontal>
                        <samples>360</samples>
                        <resolution>1</resolution>
                        <min_angle>-3</min_angle>
                        <max_angle>3</max_angle>
                    </horizontal>
                </scan>
                <range>
                    <min>0.10</min>
                    <max>30.0</max>
                    <resolution>0.01</resolution>
                </range>
                <noise>
                    <type>gaussian</type>
                    <mean>0.0</mean>
                    <stddev>0.01</stddev>
                </noise>
            </ray>
            <plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
                <topicName>/scan</topicName>
                <frameName>laser</frameName>
            </plugin>
        </sensor>
    </gazebo>
</robot>

（2）摄像头传感器 t9_sensor_camera.xacro

 新建 t9_sensor_camera.xacro

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 被引用的link -->
    <gazebo reference="camera">
        <!-- 类型设置为 camara -->
        <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</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>
</robot>

（3）修改组合文件和启动

 ①修改 t7_12346.xacro

<!-- 组合惯性矩阵文件、小车、摄像头、雷达和运动控制 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="t1_head.xacro" />
    <xacro:include filename="t2_car.xacro" />
    <xacro:include filename="t3_camera.xacro" />
    <xacro:include filename="t4_laser.xacro" />
    <xacro:include filename="t6_move.xacro" />

    <!-- 雷达仿真信息 -->
    <xacro:include filename="t8_sensor_camera.xacro" />

    <!-- 摄像头仿真信息 -->
    <xacro:include filename="t9_sensor_laser.xacro" />

</robot>

②启动

ctrl+shift+b 编译

​# 终端2
roslaunch urdf_gazebo t7_gazebo.launch

# 终端3
roslaunch urdf_gazebo t7_rviz.launch

（4）信息展示

① rviz中Add → LaserScan（设置 Topic：/scan）

② rviz中Add → Camera（设置 image topic：/camera/image_raw）

rviz窗口中显示了摄像头拍摄结果和雷达扫描结果 

3.kinect摄像头信息仿真

kinet摄像头是一种GBD深度摄像头，可以显示更多深度的拍摄信息

（1）kinet摄像头传感器 t10_sensor_kinet.xacro

新建 t10_sensor_kinet.xacro。

由于未搭建专门的组件作为kinet摄像头，因此使用小车构造中未被安排功能的支架作为摄像头。支架名称为support，因此将用到 " kinect link名称 " 的地方改为 " support "。【代码中已注释并修改过】

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- <gazebo reference="kinect link名称"> -->
    <gazebo reference="support">
        <sensor type="depth" name="camera">
            <always_on>true</always_on>
            <update_rate>20.0</update_rate>
            <camera>
                <horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
                <image>
                    <format>R8G8B8</format>
                    <width>640</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.05</near>
                    <far>8.0</far>
                </clip>
            </camera>
            <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
                <cameraName>camera</cameraName>
                <alwaysOn>true</alwaysOn>
                <updateRate>10</updateRate>
                <imageTopicName>rgb/image_raw</imageTopicName>
                <depthImageTopicName>depth/image_raw</depthImageTopicName>
                <pointCloudTopicName>depth/points</pointCloudTopicName>
                <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
                <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
                <!-- <frameName>kinect link名称</frameName> -->
                <!-- 在插件中为kinect设置坐标系，用support_depth替代support -->
                <frameName>support_depth</frameName>
                <baseline>0.1</baseline>
                <distortion_k1>0.0</distortion_k1>
                <distortion_k2>0.0</distortion_k2>
                <distortion_k3>0.0</distortion_k3>
                <distortion_t1>0.0</distortion_t1>
                <distortion_t2>0.0</distortion_t2>
                <pointCloudCutoff>0.4</pointCloudCutoff>
            </plugin>
        </sensor>
    </gazebo>
</robot>

（2）修改组合文件和启动

 ①修改 t7_12346.xacro

<!-- 组合惯性矩阵文件、小车、摄像头、雷达和运动控制 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="t1_head.xacro" />
    <xacro:include filename="t2_car.xacro" />
    <xacro:include filename="t3_camera.xacro" />
    <xacro:include filename="t4_laser.xacro" />
    <xacro:include filename="t6_move.xacro" />
    <!-- 雷达仿真信息 -->
    <xacro:include filename="t8_sensor_camera.xacro" />
    <!-- 摄像头仿真信息 -->
    <xacro:include filename="t9_sensor_laser.xacro" />
    <!-- kinect仿真信息 -->
    <xacro:include filename="t10_sensor_kinet.xacro" />
</robot>

②修改 t7_rviz.launch

<launch>
    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find urdf_gazebo)/config/t7_car.rviz" />

    <!-- 关节以及机器人状态发布节点 -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <!-- 发布新设置的坐标系,否则会错位 -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />
</launch>

②启动

ctrl+shift+b 编译

​# 终端2
roslaunch urdf_gazebo t7_gazebo.launch

# 终端3
roslaunch urdf_gazebo t7_rviz.launch

（3）信息展示

rviz中 → Camera → Image Topic ：/camera/depth/image_raw

（4）点云数据展示

在kinect中也可以以点云的方式显示感知周围环境，而不用小车在rviz中运动，操作如下：

①rviz中 → Add → Point Cloud2

②Point Cloud2 → Topic ：/camera/depth/points

【我的虚拟机同时启动rviz和gazebo，并加入各种各样的仿真信息显示组件的时候，都快爆炸了，所以一步步慢慢添加】

加入点云数据后，gazebo中小车移动时，rviz中不会移动，而是前方的 " 大屏幕 " 内容显示为拍摄到的内容