navigation2 仿真阿克曼底盘模型

navigation2 的仿真一般用 turtlebot3_waffle.urdf 模型,waffle 模型是差速轮模型,找了段时间找不到 turtlebot3 的阿克曼底盘机器,就自己造了一个模型。

gazebo 的阿克曼底盘驱动插件

查看 turtlebot3 的 waffle.model 文件知道 gazebo 有一个差速轮底盘的驱动插件叫 libgazebo_ros_diff_drive.so,可以直接接收 ros 的 cmd_vel 话题来驱动底盘模型。有差速轮驱动插件,那就应该有阿克曼驱动插件,一番折腾后在  gazebo 插件文档 中找到资料。

阿克曼的驱动插件叫 libgazebo_ros_ackermann_drive.so。参考单车模型,该驱动是后轮驱动,前轮控制航向的控制。

官方的 libgazebo_ros_ackermann_drive 配置 demo 如下:

<plugin name="gazebo_ros_ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">

  <ros>
    <namespace>demo</namespace>
    <remapping>cmd_vel:=cmd_demo</remapping>
    <remapping>odom:=odom_demo</remapping>
    <remapping>distance:=distance_demo</remapping>
  </ros>

  <update_rate>100.0</update_rate>

  <!-- wheels -->
  <front_left_joint>front_left_wheel_joint</front_left_joint>
  <front_right_joint>front_right_wheel_joint</front_right_joint>
  <rear_left_joint>rear_left_wheel_joint</rear_left_joint>
  <rear_right_joint>rear_right_wheel_joint</rear_right_joint>
  <left_steering_joint>front_left_steer_joint</left_steering_joint>
  <right_steering_joint>front_right_steer_joint</right_steering_joint>
  <steering_wheel_joint>steering_joint</steering_wheel_joint>

  <!-- Max absolute steer angle for tyre in radians-->
  <!-- Any cmd_vel angular z greater than this would be capped -->
  <max_steer>0.6458</max_steer>

  <!-- Max absolute steering angle of steering wheel -->
  <max_steering_angle>7.85</max_steering_angle>

  <!-- Max absolute linear speed in m/s -->
  <max_speed>20</max_speed>

  <!-- PID tuning -->
  <left_steering_pid_gain>1500 0 1</left_steering_pid_gain>
  <left_steering_i_range>0 0</left_steering_i_range>
  <right_steering_pid_gain>1500 0 1</right_steering_pid_gain>
  <right_steering_i_range>0 0</right_steering_i_range>
  <linear_velocity_pid_gain>1000 0 1</linear_velocity_pid_gain>
  <linear_velocity_i_range>0 0</linear_velocity_i_range>

  <!-- output -->
  <publish_odom>true</publish_odom>
  <publish_odom_tf>true</publish_odom_tf>
  <publish_wheel_tf>true</publish_wheel_tf>
  <publish_distance>true</publish_distance>

  <odometry_frame>odom_demo</odometry_frame>
  <robot_base_frame>chassis</robot_base_frame>

</plugin>

模型效果

先看模型和底盘驱动效果,后面再提供方法。

在终端发布 cmd_vel 话题:

ros2 topic pub --rate 1 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.2}}"

底盘收到话题后驱动小车向左作弧形行驶

目录结构:

注意:

urdf 模型是 rviz 中显示以及 ros 中使用的模型语言,model 是 gazebo 中使用的模型语言,语法略有区别

racecar.urdf 文件

<?xml version="1.0" ?>

<robot name="racecar">
  <!-- base footprint -->
  <link name="base_footprint"/>

  <!-- base_link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.04"/>
      <geometry>
        <box size="0.1 0.1 0.02"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.04"/>
      <geometry>
        <box size="0.1 0.1 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.04"/>
      <mass value="100"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.04" rpy="0 0 0"/>
  </joint>

  <!-- left front wheel link -->
  <link name="left_front_wheel">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="left_front_wheel_joint" type="continuous">
    <parent link="left_front_axle"/>
    <child link="left_front_wheel"/>
    <origin rpy="0 0 0" xyz="0.13 0.08 0.04"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="left_front_axle">
    <inertial>
      <origin xyz="0.13 0.08 0.04" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="left_steering_joint" type="continuous">
    <parent link="left_steering"/>
    <child link="left_front_axle"/>
    <origin rpy="0 0 0" xyz="0.13 0.08 0.05"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="left_steering">
    <inertial>
      <origin xyz="0.13 0.08 0.05" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="left_joint" type="fixed">
    <parent link="base_link"/>
    <child link="left_steering"/>
    <origin rpy="0 0 0" xyz="0.13 0.08 0.05"/>
  </joint>

  <!-- right front wheel link -->
  <link name="right_front_wheel">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="right_front_wheel_joint" type="continuous">
    <parent link="right_front_axle"/>
    <child link="right_front_wheel"/>
    <origin rpy="0 0 0" xyz="0.13 -0.08 0.04"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="right_front_axle">
    <inertial>
      <origin xyz="0.13 -0.08 0.04" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="right_steering_joint" type="continuous">
    <parent link="right_steering"/>
    <child link="right_front_axle"/>
    <origin rpy="0 0 0" xyz="0.13 -0.08 0.05"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="right_steering">
    <inertial>
      <origin xyz="0.13 -0.08 0.05" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="right_joint" type="fixed">
    <parent link="base_link"/>
    <child link="right_steering"/>
    <origin rpy="0 0 0" xyz="0.13 -0.08 0.05"/>
  </joint>

  <!-- left rear wheel link -->
  <link name="left_rear_wheel">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="left_rear_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_rear_wheel"/>
    <origin rpy="0 0 0" xyz="-0.13 0.08 0.04"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- right rear wheel link -->
  <link name="right_rear_wheel">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>
  <joint name="right_rear_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_rear_wheel"/>
    <origin rpy="0 0 0" xyz="-0.13 -0.08 0.04"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- imu -->
  <link name="imu_link"/>  
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0.0 0 0.04" rpy="0 0 0"/>
  </joint>

  <!-- scan -->
  <joint name="scan_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_scan"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
  </joint>

  <link name="base_scan">
    <inertial>
      <mass value="0.125" />
      <origin xyz="0 0 0.1" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>

</robot>

racecar.model 文件

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="default">

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <scene>
      <shadows>false</shadows>
    </scene>

    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>

    <physics type="ode">
      <real_time_update_rate>1000.0</real_time_update_rate>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <ode>
        <solver>
          <type>quick</type>
          <iters>150</iters>
          <precon_iters>0</precon_iters>
          <sor>1.400000</sor>
          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
        </solver>
        <constraints>
          <cfm>0.00001</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
          <contact_surface_layer>0.01000</contact_surface_layer>
        </constraints>
      </ode>
    </physics>

    <!-- <model name="turtlebot3_world">
      <static>1</static>
      <include>
        <uri>model://turtlebot3_world</uri>
      </include>
    </model> -->

    <model name="racecar">
      <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>

      <!-- base footprint -->
      <link name="base_footprint"/>

      <!-- base_link -->
      <link name="base_link">
        <inertial>
          <pose>0 0 0.04 0 0 0</pose>
          <!-- <inertia>
            <ixx>1.1</ixx>
            <ixy>0.000</ixy>
            <ixz>0.000</ixz>
            <iyy>1.1</iyy>
            <iyz>0.000</iyz>
            <izz>1.1</izz>
          </inertia> -->
          <mass>100</mass>
        </inertial>
        <collision name="base_link_collision">
          <pose>0 0 0.04 0 0 0</pose>
          <geometry>
            <box>
              <size>0.1 0.1 0.02</size>
            </box>
          </geometry>
        </collision>
        <visual name="base_link_visual">
          <pose>0 0 0.04 0 0 0</pose>
          <geometry>
            <box>
              <size>0.26 0.16 0.02</size>
            </box>
          </geometry>
        </visual>
      </link>
      <joint name="base_joint" type="fixed">
        <parent>base_footprint</parent>
        <child>base_link</child>
        <pose>0 0 0.04 0 0 0</pose>
      </joint>

      <!-- left front wheel link -->
      <link name="left_front_wheel">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <collision name="left_front_wheel_collision">
          <pose>0.13 0.08 0.04 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.1</mu>
                <mu2>1.1</mu2>
                <slip1>0.0</slip1>
                <slip2>0.0</slip2>
              </ode>
            </friction>
            <contact>
              <ode>
                <min_depth>0.001</min_depth>
                <kp>1e9</kp>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="left_front_wheel_visual">
          <pose>0.13 0.08 0.04 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="left_front_wheel_joint" type="revolute">
        <parent>left_front_axle</parent>
        <child>left_front_wheel</child>
        <pose>0.13 0.08 0.04 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
        </axis>
      </joint>
      <link name="left_front_axle">
        <inertial>
          <pose>0.13 0.08 0.04 0 0 0</pose>
          <mass>0.1</mass>
        </inertial>
      </link>
      <joint name="left_steering_joint" type="revolute">
        <parent>left_steering</parent>
        <child>left_front_axle</child>
        <pose>0.13 0.08 0.05 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>
      <link name="left_steering">
        <inertial>
          <pose>0.13 0.08 0.05 0 0 0</pose>
          <mass>0.1</mass>
        </inertial>
      </link>
      <joint name="left_joint" type="fixed">
        <parent>base_link</parent>
        <child>left_steering</child>
        <pose>0.13 0.08 0.05 0 0 0</pose>
      </joint>

      <!-- right front wheel link -->
      <link name="right_front_wheel">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <collision name="right_front_wheel_collision">
          <pose>0.13 -0.08 0.04 1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.1</mu>
                <mu2>1.1</mu2>
                <slip1>0.0</slip1>
                <slip2>0.0</slip2>
              </ode>
            </friction>
            <contact>
              <ode>
                <min_depth>0.001</min_depth>
                <kp>1e9</kp>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="right_front_wheel_visual">
          <pose>0.13 -0.08 0.04 1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="right_front_wheel_joint" type="revolute">
        <parent>right_front_axle</parent>
        <child>right_front_wheel</child>
        <pose>0.13 -0.08 0.04 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
        </axis>
      </joint>
      <link name="right_front_axle">
        <inertial>
          <pose>0.13 -0.08 0.04 0 0 0</pose>
          <mass>0.1</mass>
        </inertial>
      </link>
      <joint name="right_steering_joint" type="revolute">
        <parent>right_steering</parent>
        <child>right_front_axle</child>
        <pose>0.13 -0.08 0.05 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>
      <link name="right_steering">
        <inertial>
          <pose>0.13 -0.08 0.05 0 0 0</pose>
          <mass>0.1</mass>
        </inertial>
      </link>
      <joint name="right_joint" type="fixed">
        <parent>base_link</parent>
        <child>right_steering</child>
        <pose>0.13 -0.08 0.05 0 0 0</pose>
      </joint>

      <!-- front wheel link -->
      <!-- <link name="front_wheel">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <collision name="front_wheel_collision">
          <pose>0.13 0.0 0.04 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.1</mu>
                <mu2>1.1</mu2>
                <slip1>0.0</slip1>
                <slip2>0.0</slip2>
              </ode>
            </friction>
            <contact>
              <ode>
                <min_depth>0.001</min_depth>
                <kp>1e9</kp>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="front_wheel_visual">
          <pose>0.13 0.0 0.04 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="front_wheel_joint" type="revolute">
        <parent>front_axle</parent>
        <child>front_wheel</child>
        <pose>0.13 0.0 0.04 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
        </axis>
      </joint>
      <link name="front_axle">
        <inertial>
          <pose>0.13 0 0.04 0 0 0</pose>
          <mass>0.1</mass>
        </inertial>
      </link>
      <joint name="steering_joint" type="revolute">
        <parent>front_steering</parent>
        <child>front_axle</child>
        <pose>0.13 0.0 0.05 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>
      <link name="front_steering">
        <inertial>
          <pose>0.13 0 0.05 0 0 0</pose>
          <mass>0.1</mass>
        </inertial>
      </link>
      <joint name="front_joint" type="fixed">
        <parent>base_link</parent>
        <child>front_steering</child>
        <pose>0.13 0 0.05 0 0 0</pose>
      </joint> -->

      <!-- left rear wheel link -->
      <link name="left_rear_wheel">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <collision name="left_rear_wheel_collision">
          <pose>-0.13 0.08 0.04 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.1</mu>
                <mu2>1.1</mu2>
                <slip1>0.0</slip1>
                <slip2>0.0</slip2>
              </ode>
            </friction>
            <contact>
              <ode>
                <min_depth>0.001</min_depth>
                <kp>1e9</kp>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="left_rear_wheel_visual">
          <pose>-0.13 0.08 0.04 -1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="left_rear_wheel_joint" type="revolute">
        <parent>base_link</parent>
        <child>left_rear_wheel</child>
        <pose>-0.13 0.08 0.04 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
          <dynamics>
            <friction>18.0474092253</friction>
          </dynamics>
        </axis>
      </joint>

      <!-- right rear wheel link -->
      <link name="right_rear_wheel">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <collision name="right_rear_wheel_collision">
          <pose>-0.13 -0.08 0.04 1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.1</mu>
                <mu2>1.1</mu2>
                <slip1>0.0</slip1>
                <slip2>0.0</slip2>
              </ode>
            </friction>
            <contact>
              <ode>
                <min_depth>0.001</min_depth>
                <kp>1e9</kp>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="right_rear_wheel_visual">
          <pose>-0.13 -0.08 0.04 1.57 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.04</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="right_rear_wheel_joint" type="revolute">
        <parent>base_link</parent>
        <child>right_rear_wheel</child>
        <pose>-0.13 -0.08 0.04 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
          <dynamics>
            <friction>18.0474092253</friction>
          </dynamics>
        </axis>
      </joint>

      <!-- left brush -->
      <link name="left_brush">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <visual name="left_brush_visual">
          <pose>0.16 0.1 0.02 0 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.02</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="left_brush_joint" type="fixed">
        <parent>base_link</parent>
        <child>left_brush</child>
        <pose>0.16 0.1 0.01 0 0 0</pose>
      </joint>

      <!-- right brush -->
      <link name="right_brush">
        <inertial>
          <mass>1.0</mass>
        </inertial>
        <visual name="right_brush_visual">
          <pose>0.16 -0.1 0.02 0 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.02</length>
              <radius>0.04</radius>
            </cylinder>
          </geometry>
        </visual>
      </link>
      <joint name="right_brush_joint" type="fixed">
        <parent>base_link</parent>
        <child>right_brush</child>
        <pose>0.16 -0.1 0.01 0 0 0</pose>
      </joint>

      <!-- ackermann drive -->
      <plugin name="racecar_ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">

        <ros>
          <!-- <namespace>/tb3</namespace> -->
        </ros>

        <update_rate>100</update_rate>

        <!-- wheels -->
        <front_left_joint>left_front_wheel_joint</front_left_joint>
        <front_right_joint>right_front_wheel_joint</front_right_joint>
        <rear_left_joint>left_rear_wheel_joint</rear_left_joint>
        <rear_right_joint>right_rear_wheel_joint</rear_right_joint>
        <left_steering_joint>left_steering_joint</left_steering_joint>
        <right_steering_joint>right_steering_joint</right_steering_joint>
        <!-- <steering_wheel_joint>steering_joint</steering_wheel_joint> -->
        <!-- Max absolute steer angle for tyre in radians-->
        <!-- Any cmd_vel angular z greater than this would be capped -->
        <max_steer>0.6458</max_steer>
        <!-- Max absolute steering angle of steering wheel -->
        <max_steering_angle>7.85</max_steering_angle>
        <!-- Max absolute linear speed in m/s -->
        <max_speed>1.2</max_speed>
        <!-- PID tuning -->
        <left_steering_pid_gain>800 1 5</left_steering_pid_gain>
        <left_steering_i_range>0 0</left_steering_i_range>
        <right_steering_pid_gain>800 1 5</right_steering_pid_gain>
        <right_steering_i_range>0 0</right_steering_i_range>
        <linear_velocity_pid_gain>1000 0 3</linear_velocity_pid_gain>
        <linear_velocity_i_range>0 0</linear_velocity_i_range>
        <!-- output -->
        <publish_odom>true</publish_odom>
        <publish_odom_tf>true</publish_odom_tf>
        <publish_wheel_tf>false</publish_wheel_tf>
        <publish_distance>false</publish_distance>
        <odometry_topic>odom</odometry_topic>
        <odometry_frame>odom</odometry_frame>
        <robot_base_frame>base_footprint</robot_base_frame>
      </plugin>

      <plugin name="racecar_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
        <ros>
            <remapping>~/out:=joint_states</remapping>
        </ros>
        <update_rate>30</update_rate>
        <joint_name>left_front_wheel_joint</joint_name>
        <joint_name>right_front_wheel_joint</joint_name>
        <joint_name>left_rear_wheel_joint</joint_name>
        <joint_name>right_rear_wheel_joint</joint_name>
        <joint_name>left_steering_joint</joint_name>
        <joint_name>right_steering_joint</joint_name>
        <!-- <joint_name>steering_joint</joint_name> -->
      </plugin>

      <link name="base_scan">
        <inertial>
          <pose>0 0 0.1 0 0 0</pose>
          <mass>0.125</mass>
        </inertial>
        <visual name="lidar_sensor_visual">
          <pose>0 0 0.1 0 0 0</pose>
          <geometry>
            <cylinder>
              <length>0.002</length>
              <radius>0.01</radius>
            </cylinder>
          </geometry>
        </visual>
        <sensor name="hls_lfcd_lds" type="ray">
          <always_on>true</always_on>
          <visualize>true</visualize>
          <pose>0 0 0.1 0 0 0</pose>
          <update_rate>5</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>360</samples>
                <resolution>1.000000</resolution>
                <min_angle>0.000000</min_angle>
                <max_angle>6.280000</max_angle>
              </horizontal>
            </scan>
            <range>
              <min>0.120000</min>
              <max>3.5</max>
              <resolution>0.015000</resolution>
            </range>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.01</stddev>
            </noise>
          </ray>
          <plugin name="racecar_laserscan" filename="libgazebo_ros_ray_sensor.so">
            <ros>
                <!-- <namespace>/tb3</namespace> -->
                <remapping>~/out:=scan</remapping>
            </ros>
            <output_type>sensor_msgs/LaserScan</output_type>
            <frame_name>base_scan</frame_name>
          </plugin>
        </sensor>
      </link>
      <joint name="lidar_joint" type="fixed">
        <parent>base_link</parent>
        <child>base_scan</child>
        <pose>0 0 0.1 0 0 0</pose>
      </joint>
    </model>

    <model name='Wall'>
      <pose>0 0 0 0 0 0</pose>
      <link name='Wall_0'>
        <collision name='Wall_0_Collision'>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_0_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>-0 -2.925 0 0 -0 0</pose>
      </link>
      <link name='Wall_11'>
        <collision name='Wall_11_Collision'>
          <geometry>
            <box>
              <size>2 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_11_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>2 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>1.9407 1.29165 0 0 -0 0</pose>
      </link>
      <link name='Wall_3'>
        <collision name='Wall_3_Collision'>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_3_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>2.925 -0 0 0 -0 1.5708</pose>
      </link>
      <link name='Wall_4'>
        <collision name='Wall_4_Collision'>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_4_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>-0 2.925 0 0 -0 3.14159</pose>
      </link>
      <link name='Wall_5'>
        <collision name='Wall_5_Collision'>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_5_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>6 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>-2.925 -0 0 0 -0 -1.5708</pose>
      </link>
      <link name='Wall_7'>
        <collision name='Wall_7_Collision'>
          <geometry>
            <box>
              <size>4 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_7_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>4 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>-0.934225 -1.12853 0 0 -0 0</pose>
      </link>
      <link name='Wall_9'>
        <collision name='Wall_9_Collision'>
          <geometry>
            <box>
              <size>2.25 0.15 2.5</size>
            </box>
          </geometry>
          <pose>0 0 1.25 0 -0 0</pose>
        </collision>
        <visual name='Wall_9_Visual'>
          <pose>0 0 1.25 0 -0 0</pose>
          <geometry>
            <box>
              <size>2.25 0.15 2.5</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
            <ambient>1 1 1 1</ambient>
          </material>
          <meta>
            <layer>0</layer>
          </meta>
        </visual>
        <pose>-1.8489 1.25198 0 0 -0 0</pose>
      </link>
      <static>1</static>
    </model>
  </world>
</sdf>

参考

gazebo 官方插件文档

  • 1
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 4
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值