机器人xacro设计+gazebo/rviz启动

项目需求

创建一个机器人，包含本体、控制器、传感器。在gazebo物理引擎中加载真实世界模型，在rviz通过各种插件查看状态。

方案

首先，看一下整体的项目截图

urdf02_gazebo功能包下面有/config文件夹放置rviz的配置文件，/launch文件夹放置启动文件，/world文件夹放置gazebo的物理世界模型，/xacro文件夹放机器人模型文件，分别在/xacro当前目录下放机器人本体的xacro文件，以及在/xacro/gazebo文件夹下放控制和传感器文件，最终所有的xacro文件都集成进/xacro/car_gazebo.xacro中，即car_gazebo.xacro集成了机器人本体、控制器、传感器，值得注意的是，xacro集成了这么多个不同的 xacro文件，其本质上是将所有的xacro文件复制到一块的，所以不同的xacro文件可写同一个name，反正最终都会复制集成到同一个xacro文件下，car_gazebo.xacro如下所示。

机器人的本体设计

机器人本体集成car_gazebo.xacro

<robot name="car" xmlns:xacro="http://wiki.ros.org/xacro"> 
    <xacro:include filename="head.xacro" />
    <xacro:include filename="base.xacro" />
    <xacro:include filename="camera.xacro" />
    <xacro:include filename="laser.xacro" />
    
    <xacro:include filename="gazebo/move.xacro" />
    <xacro:include filename="gazebo/laser_sensor.xacro" />
    <xacro:include filename="gazebo/camera_sensor.xacro" />
robot>

其中第一个head.xacro是计算inertial惯性矩阵的，因为gazebo是需要考虑真实物理引擎的，所以head.xacro里封装了宏（相当于函数，包含函数名和参数），每个宏函数都是标准的物体，如球，圆柱等，传参传入质量，长宽高半径等，宏函数就会自动计算惯性矩阵，宏函数在机器人本体设计直接调用即可。

inertial惯性矩阵 head.xacro

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    
    <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>

以下是各个机器人本体连杆link的设计，包括小车底盘、摄像头、雷达，其中的inertial惯性矩阵用到了head.xacro的宏函数。

小车底盘base.xacro

<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    
    
    <xacro:property name="PI" value="3.1415926"/>
    
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    material>
    
    <xacro:property name="base_footprint_radius" value="0.001" /> 
    <xacro:property name="base_link_radius" value="0.1" /> 
    <xacro:property name="base_link_length" value="0.08" /> 
    <xacro:property name="earth_space" value="0.015" /> 
    <xacro:property name="base_link_m" value="0.5" /> 

    
    <link name="base_footprint">
      <visual>
        <geometry>
          <sphere radius="${base_footprint_radius}" />
        geometry>
      visual>
    link>

    <link name="base_link">
      <visual>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        material>
      visual>
      <collision>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
      collision>
      <xacro:cylinder_inertial_matrix m="${base_link_m}" r="${base_link_radius}" h="${base_link_length}" />

    link>


    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
    joint>
    <gazebo reference="base_link">
        <material>Gazebo/Yellowmaterial>
    gazebo>

    
    
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.015" />
    <xacro:property name="wheel_m" value="0.05" /> 

    
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        visual>
        <collision>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
        collision>
        <xacro:cylinder_inertial_matrix m="${wheel_m}" r="${wheel_radius}" h="${wheel_length}" />

      link>

      <joint name="${name}_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      joint>

      <gazebo reference="${name}_wheel">
        <material>Gazebo/Redmaterial>
      gazebo>

    xacro:macro>
    <xacro:add_wheels name="left" flag="1" />
    <xacro:add_wheels name="right" flag="-1" />
    
    
    <xacro:property name="support_wheel_radius" value="0.0075" /> 
    <xacro:property name="support_wheel_m" value="0.03" /> 

    
    <xacro:macro name="add_support_wheel" params="name flag" >
      <link name="${name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black" />
        visual>
        <collision>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
        collision>
        <xacro:sphere_inertial_matrix m="${support_wheel_m}" r="${support_wheel_radius}" />
      link>

      <joint name="${name}_wheel2base_link" type="continuous">
          <parent link="base_link" />
          <child link="${name}_wheel" />
          <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
          <axis xyz="1 1 1" />
      joint>
      <gazebo reference="${name}_wheel">
        <material>Gazebo/Redmaterial>
      gazebo>
    xacro:macro>

    <xacro:add_support_wheel name="front" flag="1" />
    <xacro:add_support_wheel name="back" flag="-1" />
robot>

摄像头camera.xacro

<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    
    <xacro:property name="camera_length" value="0.01" /> 
    <xacro:property name="camera_width" value="0.025" /> 
    <xacro:property name="camera_height" value="0.025" /> 
    <xacro:property name="camera_x" value="0.08" /> 
    <xacro:property name="camera_y" value="0.0" /> 
    <xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> 

    <xacro:property name="camera_m" value="0.01" /> 

    
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        visual>
        <collision>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        collision>
        <xacro:Box_inertial_matrix m="${camera_m}" l="${camera_length}" w="${camera_width}" h="${camera_height}" />
    link>

    <joint name="camera2base_link" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_x} ${camera_y} ${camera_z}" />
    joint>
    <gazebo reference="camera">
        <material>Gazebo/Bluematerial>
    gazebo>
robot>

雷达laser.xacro

<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">

    
    <xacro:property name="support_length" value="0.15" /> 
    <xacro:property name="support_radius" value="0.01" /> 
    <xacro:property name="support_x" value="0.0" /> 
    <xacro:property name="support_y" value="0.0" /> 
    <xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> 

    <xacro:property name="support_m" value="0.02" /> 

    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red">
                <color rgba="0.8 0.2 0.0 0.8" />
            material>
        visual>

        <collision>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        collision>

        <xacro:cylinder_inertial_matrix m="${support_m}" r="${support_radius}" h="${support_length}" />

    link>

    <joint name="support2base_link" type="fixed">
        <parent link="base_link" />
        <child link="support" />
        <origin xyz="${support_x} ${support_y} ${support_z}" />
    joint>

    <gazebo reference="support">
        <material>Gazebo/Whitematerial>
    gazebo>

    
    <xacro:property name="laser_length" value="0.05" /> 
    <xacro:property name="laser_radius" value="0.03" /> 
    <xacro:property name="laser_x" value="0.0" /> 
    <xacro:property name="laser_y" value="0.0" /> 
    <xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> 

    <xacro:property name="laser_m" value="0.1" /> 

    
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        visual>
        <collision>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        collision>
        <xacro:cylinder_inertial_matrix m="${laser_m}" r="${laser_radius}" h="${laser_length}" />
    link>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser" />
        <origin xyz="${laser_x} ${laser_y} ${laser_z}" />
    joint>
    <gazebo reference="laser">
        <material>Gazebo/Blackmaterial>
    gazebo>
robot>

以上是机器人本体的连杆link部件设计，那么小车还需要控制器还有上面的传感器真实引用，关于这部分内容我放在了/gazebo文件下。

机器joint关节控制器+传感器sensor设计

控制器也是通过xacro文件实现，小车动起来主要是通过joint关节传动，比如小车左轮与地盘连接的joint关节，控制关节转动即可，由于小车有两个驱动轮，所以joint驱动也封装成宏函数来实现。

机器人joint关节控制器move.xacro

<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">

    
    <xacro:macro name="joint_trans" params="joint_name">
        
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmissiontype>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
            joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
                <mechanicalReduction>1mechanicalReduction>
            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>DebugrosDebugLevel>
            <publishWheelTF>truepublishWheelTF>
            <robotNamespace>/robotNamespace>
            <publishTf>1publishTf>
            <publishWheelJointState>truepublishWheelJointState>
            <alwaysOn>truealwaysOn>
            <updateRate>100.0updateRate>
            <legacyMode>truelegacyMode>
            <leftJoint>left_wheel2base_linkleftJoint> 
            <rightJoint>right_wheel2base_linkrightJoint> 
            <wheelSeparation>${base_link_radius * 2}wheelSeparation> 
            <wheelDiameter>${wheel_radius * 2}wheelDiameter> 
            <broadcastTF>1broadcastTF>
            <wheelTorque>30wheelTorque>
            <wheelAcceleration>1.8wheelAcceleration>
            <commandTopic>cmd_velcommandTopic> 
            <odometryFrame>odomodometryFrame> 
            <odometryTopic>odomodometryTopic> 
            <robotBaseFrame>base_footprintrobotBaseFrame> 
        plugin>
    gazebo>

robot>

接下来是传感器的配置，因为机器人本体已经有了camera连杆和laser连杆，所以现在需要给这两个存在的连杆配上传感器，也就是将传感器映射到连杆中。引用方式是gazebo reference=“连杆名”，连杆名在机器人本体连杆文件中，保持一致。

摄像头传感器camera_sensor.xacro

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
  
  <gazebo reference="camera">
    
    <sensor type="camera" name="camera_node">
      <update_rate>30.0update_rate> 
      
      <camera name="head">
        <horizontal_fov>1.3962634horizontal_fov>
        <image>
          <width>1280width>
          <height>720height>
          <format>R8G8B8format>
        image>
        <clip>
          <near>0.02near>
          <far>300far>
        clip>
        <noise>
          <type>gaussiantype>
          <mean>0.0mean>
          <stddev>0.007stddev>
        noise>
      camera>
      
      <plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
        <alwaysOn>truealwaysOn>
        <updateRate>0.0updateRate>
        <cameraName>/cameracameraName>
        <imageTopicName>image_rawimageTopicName>
        <cameraInfoTopicName>camera_infocameraInfoTopicName>
        <frameName>cameraframeName>
        <hackBaseline>0.07hackBaseline>
        <distortionK1>0.0distortionK1>
        <distortionK2>0.0distortionK2>
        <distortionK3>0.0distortionK3>
        <distortionT1>0.0distortionT1>
        <distortionT2>0.0distortionT2>
      plugin>
    sensor>
  gazebo>
robot>

雷达传感器laser_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 0pose>
      <visualize>truevisualize>
      <update_rate>5.5update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360samples>
            <resolution>1resolution>
            <min_angle>-3min_angle>
            <max_angle>3max_angle>
          horizontal>
        scan>
        <range>
          <min>0.10min>
          <max>30.0max>
          <resolution>0.01resolution>
        range>
        <noise>
          <type>gaussiantype>
          <mean>0.0mean>
          <stddev>0.01stddev>
        noise>
      ray>
      <plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
        <topicName>/scantopicName>
        <frameName>laserframeName>
      plugin>
    sensor>
  gazebo>

robot>

好了，终于完成了机器人的整体设计了，现在机器人有了本体，还有了两轮差速控制器，还有了传感器。

接下来就让机器人通过launch文件启动gazebo和rviz中吧。

launch文件启动gazebo和rviz

分别通过launch来启动gazebo和rviz，有启动先后顺序的，因为car_gazebo.xacro先在car_gazebo.launch中加载到参数服务器中，所以要先启动car_gazebo.launch，并且在car_rviz.launch不需要加载car_gazebo.xacro文件，因为car_gazebo.launch已经将其加载参数服务器了。

car_gazebo.launch

<launch>
    
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/xacro/car_gazebo.xacro" />

    
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find urdf02_gazebo)/world/box_house.world" />
    include>

    
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
launch>

car_rviz.launch

<launch>
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf02_gazebo)/config/show.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>

参考来源
【Autolabor初级教程】ROS机器人入门
URDF集成Gazebo