2021.8.20~8.24---6.6、6.7、6.8/9---机器人系统仿真之URDF集成Gazebo、URDF、Gazebo与Rviz综合应用、本章小结

一、URDF集成Gazebo

1.URDF与Gazebo基本集成流程

1）创建功能包

2）编写URDF文件

URDF文件

<robot name="mycar">
        <link name="base_link">
                <!-- 可视化部分 -->
                <visual>
                        <geometry>
                                <box size="0.5 0.3 0.1"/>
                        </geometry>
                        <origin xyz="0 0 0" rpy="0 0 0" />
                        <material name="yellow">
                                <color rgba="0.5 0.3 0 0.5" />
                        </material>
                </visual>
                <!-- 1.设置碰撞参数 -->
                <!-- 如果是标准几何体，直接复制 visual 的 geometry 和 origin 即可 -->
                <collision>
                         <geometry>
                                <box size="0.5 0.3 0.1"/>
                        </geometry>
                        <origin xyz="0 0 0" rpy="0 0 0" />
                </collision>

                <!-- 2.设置惯性矩阵 -->
                <inertial>
                        <origin xyz="0 0 0" />
                        <mass value="2" />
                        <inertia ixx="1" ixy="0" ixz="0" iyy="0" iyz="1" izz="1"/>
                </inertial>
        </link>
        <!-- gazebo 有自己的颜色设置标签 -->
        <gazebo reference="base_link">
                <material>Gazebo/Red</material>
        </gazebo>
</robot>

3）启动Gezebo并显示模型

launch文件

<launch>
        <!-- 1.需要在参数服务器中载入 urdf -->
        <param name="robot_description" textfile="$(find urdf02_gazebo)/urdf/demo01_helloworld.urdf" />
        <!-- 2.启动 gazebo 仿真环境 -->
        <include file="$(find gazebo_ros)/launch/empty_world.launch" />
        <!-- 3. 在 gazebo 中添加机器人模型-->
        <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model car -param robot_description" />
</launch>

2.URDF集成Gazebo相关设置

球体惯性矩阵

<!-- 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 l w h">
    <inertial>
        <mass value="${m}" />
        <inertia ixx="${m*(h*h + l*l)/12}" ixy="0" ixz="0" iyy="${m*(w*w + l*l)/12}" iyz="0" izz="${m*(w*w + h*h)/12}" />
    </inertial>
</xacro:macro>

3.URDF集成Gazebo实操

1）编写封装惯性矩阵算法的xacro文件

head.xacro

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 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 l w h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(h*h + l*l)/12}" ixy="0" ixz="0" iyy="${m*(w*w + l*l)/12}" iyz="0" izz="${m*(w*w + h*h)/12}" />
        </inertial>
    </xacro:macro>
</robot>

2）URDF文件集成

car.urdf.xacro文件

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
        <!-- 包含惯性矩阵文件 -->
        <xacro:include filename="head.xacro" />
        <!-- 包含底盘、摄像头与雷达的 xacro 文件 -->
        <xacro:include filename="demo05_car_base.urdf.xacro" />
        <xacro:include filename="demo06_car_camera.urdf.xacro" />
        <xacro:include filename="demo07_car_laser.urdf.xacro" />
</robot >

4.Gazebo仿真环境搭建

1）添加内置组件创建仿真环境

launch文件

<launch>
        <!-- 1.需要在参数服务器中载入 urdf -->
        <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
        <!-- 2.启动 gazebo 仿真环境 -->
        <include file="$(find gazebo_ros)/launch/empty_world.launch">
                <arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" />
        </include>
        <!-- 3. 在 gazebo 中添加机器人模型-->
        <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model car -param robot_description" />
</launch>

2）自定义仿真环境

二、URDF、Gazebo与Rviz综合应用

1.机器人运动控制以及历程及信息显示

1）ros_control简介

2）运动控制实现流程（Gazebo）

·1 为joint添加传动装置以及控制器

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="left2link" />
    <xacro:joint_trans joint_name="right2link" />
 
    <!-- 控制器 -->
    <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>left2link</leftJoint> <!-- 左轮 -->
            <rightJoint>right2link</rightJoint> <!-- 右轮 -->
            <wheelSeparation>${base_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文件集成

car.urdf.xacro文件

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
        <!-- 包含惯性矩阵文件 -->
        <xacro:include filename="head.xacro" />
        <!-- 包含底盘、摄像头与雷达的 xacro 文件 -->
        <xacro:include filename="demo05_car_base.urdf.xacro" />
        <xacro:include filename="demo06_car_camera.urdf.xacro" />
        <-- 运动控制 -->
        <xacro:include filename="demo07_car_laser.urdf.xacro" />
</robot >

·3 启动gazebo并控制机器人运动

env.launch文件

<launch>
        <!-- 1.需要在参数服务器中载入 urdf -->
        <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
        <!-- 2.启动 gazebo 仿真环境 -->
        <include file="$(find gazebo_ros)/launch/empty_world.launch">
                <arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" />
        </include>
        <!-- 3. 在 gazebo 中添加机器人模型-->
        <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model car -param robot_description" />
</launch>

3）Rviz查看里程计信息

·1 启动Rviz

sensor.launch文件

<launch>
        <!-- 启动 rviz -->
        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
         <!-- 添加关节状态发布节点 -->
        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
         <!-- 添加机器人状态发布节点 -->
        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
</launch>

·2 添加组件

2.雷达信息仿真以及显示

sensor.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>

3.摄像头信息仿真以及显示

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>

4.kinect信息仿真以及显示

kinect.xacro文件

<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</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>

补充：kinect 点云数据显示

修改kinect.xacro文件

<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>

修改sensor.launch文件

<launch>
        <!-- 添加点云坐标系到kinect连杆坐标系的变换 -->
        <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth "/>
        <!-- 启动 rviz -->
        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
         <!-- 添加关节状态发布节点 -->
        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
         <!-- 添加机器人状态发布节点 -->
        <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
</launch>

5.URDF文件集成

car.urdf.xacro文件

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
        <xacro:include filename="head.xacro" />
        <xacro:include filename="demo05_car_base.urdf.xacro" />
        <xacro:include filename="demo06_car_camera.urdf.xacro" />
        <xacro:include filename="demo07_car_laser.urdf.xacro" />
        <xacro:include filename="gazebo/move.xacro" /> 
        <xacro:include filename="gazebo/laser.xacro" /> 
        <xacro:include filename="gazebo/camera.xacro" /> 
        <xacro:include filename="gazebo/kinect.xacro" /> 

</robot >

三、本章小结