ROS仿真笔记之——gazebo配置velodyne

Posted 2021-10-07 gwpscut

之前博客《ROS仿真笔记之——基于gazebo的event camera仿真(dvs gazebo)》介绍了在gazebo里面配置dvs sensor，本博文将velodyne也加上，并且在dvs camera里面加入image，使得仿真的event camera在输出event stream的同时，也可以输出图片。

首先把需要用到的so文件放在一个文件夹中，方便使用

然后更改机器人的.urdf.xacro文件，把VLP-16 sensor加进去

<!-- 加入激光雷达 -->
    <xacro:include filename="/home/kwanwaipang/gazebo_so/VLP_16_model/VLP-16.urdf.xacro"/>
  <xacro:VLP-16 parent="base_link" name="velodyne" topic="/velodyne_points" hz="10" samples="440" gpu="${gpu}">
    <origin xyz="0 0 0.2" rpy="0 0 0" />
  </xacro:VLP-16>

注意：对应的VLP-16.urdf.xacro文件里面的路径需要更改

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="VLP-16">
  <xacro:property name="M_PI" value="3.1415926535897931" />
  <xacro:macro name="VLP-16" params="*origin parent:=base_link name:=velodyne topic:=/velodyne_points hz:=10 lasers:=16 samples:=1875 collision_range:=0.3 min_range:=0.9 max_range:=130.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} gpu:=false">

    <joint name="${name}_base_mount_joint" type="fixed">
      <xacro:insert_block name="origin" /> 
      <parent link="${parent}"/>
      <child link="${name}_base_link"/>
    </joint>

    <link name="${name}_base_link">
      <inertial>
        <mass value="0.83"/>
        <origin xyz="0 0 0.03585"/>
        <inertia ixx="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" ixy="0" ixz="0"
          iyy="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" iyz="0"
          izz="${0.5 * 0.83 * (0.0516*0.0516)}"/>
      </inertial>
      <visual>
        <geometry>
          <mesh filename="/home/kwanwaipang/gazebo_so/VLP_16_model/meshes/VLP16_base_1.dae" />
        </geometry>
      </visual>
      <visual>
        <geometry>
          <mesh filename="//home/kwanwaipang/gazebo_so/VLP_16_model/meshes/VLP16_base_2.dae" />
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.03585"/>
        <geometry>
          <cylinder radius="0.0516" length="0.0717"/>
        </geometry>
      </collision>
    </link>

    <joint name="${name}_base_scan_joint" type="fixed" >
      <origin xyz="0 0 0.0377" rpy="0 0 0" />
      <parent link="${name}_base_link" />
      <child link="${name}"/>
    </joint>

    <link name="${name}">
      <inertial>
        <mass value="0.01"/>
        <origin xyz="0 0 0"/>
        <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
      </inertial>
      <visual>
        <origin xyz="0 0 -0.0377" />
        <geometry>
          <mesh filename="//home/kwanwaipang/gazebo_so/VLP_16_model/meshes/VLP16_scan.dae" />
        </geometry>
      </visual>
    </link>

    <!-- Gazebo requires the velodyne_gazebo_plugins package -->
    <gazebo reference="${name}">
      <xacro:if value="${gpu}">
        <sensor type="gpu_ray" name="${name}-VLP16">
          <pose>0 0 0 0 0 0</pose>
          <visualize>false</visualize>
          <update_rate>${hz}</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>${samples}</samples>
                <resolution>1</resolution>
                <min_angle>${min_angle}</min_angle>
                <max_angle>${max_angle}</max_angle>
              </horizontal>
              <vertical>
                <samples>${lasers}</samples>
                <resolution>1</resolution>
                <min_angle>-${15.0*M_PI/180.0}</min_angle>
                <max_angle> ${15.0*M_PI/180.0}</max_angle>
              </vertical>
            </scan>
            <range>
              <min>${collision_range}</min>
              <max>${max_range+1}</max>
              <resolution>0.001</resolution>
            </range>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.0</stddev>
            </noise>
          </ray>
          <plugin name="gazebo_ros_laser_controller" filename="/home/kwanwaipang/gazebo_so/libgazebo_ros_velodyne_gpu_laser.so">
            <topicName>${topic}</topicName>
            <frameName>${name}</frameName>
            <min_range>${min_range}</min_range>
            <max_range>${max_range}</max_range>
            <gaussianNoise>${noise}</gaussianNoise>
          </plugin>
        </sensor>
      </xacro:if>
      <xacro:unless value="${gpu}">
        <sensor type="ray" name="${name}-VLP16">
          <pose>0 0 0 0 0 0</pose>
          <visualize>false</visualize>
          <update_rate>${hz}</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>${samples}</samples>
                <resolution>1</resolution>
                <min_angle>${min_angle}</min_angle>
                <max_angle>${max_angle}</max_angle>
              </horizontal>
              <vertical>
                <samples>${lasers}</samples>
                <resolution>1</resolution>
                <min_angle>-${15.0*M_PI/180.0}</min_angle>
                <max_angle> ${15.0*M_PI/180.0}</max_angle>
              </vertical>
            </scan>
            <range>
              <min>${collision_range}</min>
              <max>${max_range+1}</max>
              <resolution>0.001</resolution>
            </range>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.0</stddev>
            </noise>
          </ray>
          <plugin name="gazebo_ros_laser_controller" filename="/home/kwanwaipang/gazebo_so/libgazebo_ros_velodyne_laser.so">
            <topicName>${topic}</topicName>
            <frameName>${name}</frameName>
            <min_range>${min_range}</min_range>
            <max_range>${max_range}</max_range>
            <gaussianNoise>${noise}</gaussianNoise>
          </plugin>
        </sensor>
      </xacro:unless>
    </gazebo>

  </xacro:macro>
</robot>

更改对应的gazebo.xacro文件，把该加进去的sensor.so文件加进去

 <!-- camera -->
  <gazebo reference="camera_rgb_frame">
    <sensor type="camera" name="camera1">
      <!-- <update_rate>30.0</update_rate> -->
      <camera name='__default__'>
        <horizontal_fov>1.8</horizontal_fov>
        <image>
          <width>128</width>
           <height>128</height>
          <!-- <format>R8G8B8</format> -->
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <!-- <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise> -->
      </camera>
      <update_rate>60</update_rate>
        <alwaysOn>1</alwaysOn>
         <visualize>0</visualize>
      <plugin name="camera_controller" filename="/home/kwanwaipang/catkin_ws/devel/lib/libgazebo_dvs_plugin.so">
        <!-- <alwaysOn>true</alwaysOn> -->
        <!-- <alwaysOn>1</alwaysOn> -->
        <!-- <update_rate>60</update_rate> -->
         <!-- <visualize>0</visualize> -->
        <!-- <updateRate>0.0</updateRate> -->
        <eventThreshold>8</eventThreshold>
        <cameraName>camera1</cameraName>
        <robotNamespace>/</robotNamespace>
        <!-- <imageTopicName>image_raw</imageTopicName> -->
        <eventsTopicName>events</eventsTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <!-- <frameName>camera_rgb_optical_frame</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>

<!-- 把普通的camera也加入 -->
       <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <cameraName>camera1</cameraName>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <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>

对应的davis_mono.launch如下

<launch>
  <!-- camera driver -->
  <node name="davis_ros_driver" pkg="davis_ros_driver" type="davis_ros_driver" />

  <!-- visualization -->
  <node name="dvs_renderer" pkg="dvs_renderer" type="dvs_renderer">
    <!-- <param name="display_method" value="grayscale"/> -->
    <param name="display_method" value="red-blue"/>
    <!-- <remap from="events" to="/dvs/events" /> -->
    <remap from="events" to="/camera1/events" />
    <!-- <remap from="image" to="/dvs/image_raw" /> -->
     <remap from="image" to="/camera1/rgb/image_raw" />
    <!-- <remap from="camera_info" to="/dvs/camera_info" /> -->
      <remap from="camera_info" to="/camera1/rgb/camera_info" />
  </node>

  <!-- display -->
  <node name="image_view" pkg="rqt_image_view" type="rqt_image_view">
  	<remap from="image" to="dvs_rendering"/>
  </node>

  <!-- configure -->
  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />

</launch>

然后运行(注意xacro里面有中文可能会导致运行不了～)

roslaunch turtlebot3_navigation dvs_test.launch
rosrun turtlebot3_teleop turtlebot3_teleop_key
rostopic echo camera1/event1
roslaunch dvs_renderer davis_mono.launch
rosrun rviz rviz

图片效果：

视频效果：

dvs