< >
Home » ROS2探索总结 » ROS2探索总结-11.ROS 2 gazebo仿真入门础

ROS2探索总结-11.ROS 2 gazebo仿真入门础

ROS2探索总结-11.ROS 2 gazebo仿真入门

说明:

  • gazebo是ROS中常用的三维物理仿真环境,在ROS 2中已经支持,这里我们就来小试牛刀。

前提条件:

  • 已经安装了ROS 2和gazebo 9(或者ROS melodic)

  • 安装接口

$ sudo apt install ros-crystal-gazebo-ros-pkgs

一、gazebo例程

$ gazebo --verbose /opt/ros/crystal/share/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world
  • 终端中可以看到如下加载信息:

请输入图片描述

  • gazebo仿真环境很快就会打开:

请输入图片描述

  • 想要看看系统有没有跑起来,关键是看话题有没有发布/订阅:

请输入图片描述

  • 可以看到,其中有一个cmd_demo的话题,相当于ROS 1中的cmd_vel,发布该话题就可以让gazebo中的小车模型动起来了:
$ ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{linear: {x: 1.0}}' -1
  • 话题列表中还有一个odom_demo,可以实时反馈小车的里程计信息:

请输入图片描述

二、探究仿真背后的内容

  • gazebo仿真例程跑起来了,各种数据我们也都可以看到,回过头来,我们再看下运行以上例程的文件gazebo_ros_diff_drive_demo.world,该文件可以在以下路径中找到:

请输入图片描述

  • gazebo_ros_diff_drive_demo.world文件内容并不算多,主要是sdf模型文件的描述。

  • 头部注释里是该例程的使用方法,大家也都可以试一下:

Gazebo ROS differential drive plugin demo
 
  Try sending commands:
 
    ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{linear: {x: 1.0}}' -1
 
    ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{angular: {z: 0.1}}' -1
 
  Try listening to odometry:
 
    ros2 topic echo /demo/odom_demo
 
  Try listening to TF:
 
    ros2 run tf2_ros tf2_echo odom chassis
 
    ros2 run tf2_ros tf2_echo chassis right_wheel
 
    ros2 run tf2_ros tf2_echo chassis left_wheel
  • 接下来是sdf模型相关的描述,除了ground和sun模型的调用外,重点是小车模型vehicle的描述,和URDF建模的语法差别不大:
 <world name="default">

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

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

<model name='vehicle'>
  <pose>0 0 0.325 0 -0 0</pose>

  <link name='chassis'>
    <pose>-0.151427 -0 0.175 0 -0 0</pose>
    <inertial>
      <mass>1.14395</mass>
      <inertia>
        <ixx>0.126164</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.416519</iyy>
        <iyz>0</iyz>
        <izz>0.481014</izz>
      </inertia>
    </inertial>
    <visual name='visual'>
      <geometry>
        <box>
          <size>2.01142 1 0.568726</size>
        </box>
      </geometry>
    </visual>
    <collision name='collision'>
      <geometry>
        <box>
          <size>2.01142 1 0.568726</size>
        </box>
      </geometry>
    </collision>
  </link>

  <link name='left_wheel'>
    <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
    <inertial>
      <mass>2</mass>
      <inertia>
        <ixx>0.145833</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.145833</iyy>
        <iyz>0</iyz>
        <izz>0.125</izz>
      </inertia>
    </inertial>
    <visual name='visual'>
      <geometry>
        <sphere>
          <radius>0.3</radius>
        </sphere>
      </geometry>
    </visual>
    <collision name='collision'>
      <geometry>
        <sphere>
          <radius>0.3</radius>
        </sphere>
      </geometry>
      <surface>
        <friction>
          <ode>
            <mu>1</mu>
            <mu2>1</mu2>
            <slip1>0</slip1>
            <slip2>0</slip2>
          </ode>
        </friction>
        <contact>
          <ode>
            <soft_cfm>0</soft_cfm>
            <soft_erp>0.2</soft_erp>
            <kp>1e+13</kp>
            <kd>1</kd>
            <max_vel>0.01</max_vel>
            <min_depth>0.01</min_depth>
          </ode>
        </contact>
      </surface>
    </collision>
  </link>

  <link name='right_wheel'>
    <pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
    <inertial>
      <mass>2</mass>
      <inertia>
        <ixx>0.145833</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.145833</iyy>
        <iyz>0</iyz>
        <izz>0.125</izz>
      </inertia>
    </inertial>
    <visual name='visual'>
      <geometry>
        <sphere>
          <radius>0.3</radius>
        </sphere>
      </geometry>
    </visual>
    <collision name='collision'>
      <geometry>
        <sphere>
          <radius>0.3</radius>
        </sphere>
      </geometry>
      <surface>
        <friction>
          <ode>
            <mu>1</mu>
            <mu2>1</mu2>
            <slip1>0</slip1>
            <slip2>0</slip2>
          </ode>
        </friction>
        <contact>
          <ode>
            <soft_cfm>0</soft_cfm>
            <soft_erp>0.2</soft_erp>
            <kp>1e+13</kp>
            <kd>1</kd>
            <max_vel>0.01</max_vel>
            <min_depth>0.01</min_depth>
          </ode>
        </contact>
      </surface>
    </collision>
  </link>

  <link name='caster'>
    <pose>-0.957138 -0 -0.125 0 -0 0</pose>
    <inertial>
      <mass>1</mass>
      <inertia>
        <ixx>0.1</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.1</iyy>
        <iyz>0</iyz>
        <izz>0.1</izz>
      </inertia>
    </inertial>
    <visual name='visual'>
      <geometry>
        <sphere>
          <radius>0.2</radius>
        </sphere>
      </geometry>
    </visual>
    <collision name='collision'>
      <geometry>
        <sphere>
          <radius>0.2</radius>
        </sphere>
      </geometry>
    </collision>
  </link>

  <joint name='left_wheel_joint' type='revolute'>
    <parent>chassis</parent>
    <child>left_wheel</child>
    <axis>
      <xyz>0 0 1</xyz>
      <limit>
        <lower>-1.79769e+308</lower>
        <upper>1.79769e+308</upper>
      </limit>
    </axis>
  </joint>

  <joint name='right_wheel_joint' type='revolute'>
    <parent>chassis</parent>
    <child>right_wheel</child>
    <axis>
      <xyz>0 0 1</xyz>
      <limit>
        <lower>-1.79769e+308</lower>
        <upper>1.79769e+308</upper>
      </limit>
    </axis>
  </joint>

  <joint name='caster_wheel' type='ball'>
    <parent>chassis</parent>
    <child>caster</child>
  </joint>
  • 重点是最下边的差速控制器插件配置,其中的参数配置和ROS 1是一致的:
   <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>

    <ros>
      <namespace>/demo</namespace>
      <argument>cmd_vel:=cmd_demo</argument>
      <argument>odom:=odom_demo</argument>
    </ros>

    <!-- wheels -->
    <left_joint>left_wheel_joint</left_joint>
    <right_joint>right_wheel_joint</right_joint>

    <!-- kinematics -->
    <wheel_separation>1.25</wheel_separation>
    <wheel_diameter>0.6</wheel_diameter>

    <!-- limits -->
    <max_wheel_torque>20</max_wheel_torque>
    <max_wheel_acceleration>1.0</max_wheel_acceleration>

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

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

  </plugin>

三、更多仿真例程

  • 在以上例程同样的路径下,还可以看到不少其他gazebo仿真例程,包含了多种传感器和常用功能:

请输入图片描述

  • 这里仅以其中一个传感器的demo为例进行演示:

    $ gazebo --verbose /opt/ros/crystal/share/gazebo_plugins/worlds/gazebo_ros_ray_sensor_demo.world

请输入图片描述

  • 启动后可以看到如下gazebo界面:

请输入图片描述

  • 该仿真包含的传感器有:激光、点云、声纳。

请输入图片描述

  • 在rviz中可以看到这些传感器的可视化效果:

请输入图片描述

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2探索总结