gazebo中添加动态障碍物

gazebo 教程

1. gazebo github
   https://github.com/gazebosim/gazebo-classic/tree/gazebo9
2. gazebo tutorials
   https://classic.gazebosim.org/tutorials
3. 运行一个空白环境

<sdf version="1.4">
  <world name="default">

    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>true</shadows>
    </scene>

    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>

    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
 
  </world>

</sdf>

gazebo 添加动态障碍物

参照官方教程：
https://classic.gazebosim.org/tutorials?tut=actor&cat=build_robot

actor标签范围内的模型配置。人会在多点间运动

<sdf version="1.6">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>
    <actor name="actor">
      <skin>
        <filename>walk.dae</filename>
      </skin>
      <animation name="animation">
        <filename>walk.dae</filename>
      </animation>
      <script>
        <trajectory id="0" type="walking">
          <waypoint>
            <time>0</time>
            <pose>0 2 0 0 0 -1.57</pose>
          </waypoint>
          <waypoint>
            <time>2</time>
            <pose>0 -2 0 0 0 -1.57</pose>
          </waypoint>
          <waypoint>
            <time>2.5</time>
            <pose>0 -2 0 0 0 1.57</pose>
          </waypoint>
          <waypoint>
            <time>7</time>
            <pose>0 2 0 0 0 1.57</pose>
          </waypoint>
          <waypoint>
            <time>7.5</time>
            <pose>0 2 0 0 0 -1.57</pose>
          </waypoint>
        </trajectory>
      </script>
    </actor>
  </world>
</sdf>

问题： 这个人或其它模型的配置是透明的，激光无法监测到对应的物体

gazebo添加动态障碍物插件

参照： https://zhuanlan.zhihu.com/p/404197579
https://blog.csdn.net/allenhsu6/article/details/114068662

默认gazebo9 的插件路径： /usr/lib/x86_64-linux-gnu/gazebo-9/plugins

kint@kint:~$ ls /usr/lib/x86_64-linux-gnu/gazebo-9/plugins 
libActorPlugin.so           libCameraPlugin.so       libFiducialCameraPlugin.so  libHydraPlugin.so            libLinearBatteryConsumerPlugin.so  libRandomVelocityPlugin.so  libSkidSteerDrivePlugin.so
libActuatorPlugin.so        libCartDemoPlugin.so     libFollowerPlugin.so        libImuSensorPlugin.so        libLinearBatteryPlugin.so          libRayPlugin.so             libSonarPlugin.so
libArduCopterPlugin.so      libCessnaGUIPlugin.so    libForceTorquePlugin.so     libInitialVelocityPlugin.so  libLinkPlot3DPlugin.so             libRaySensorNoisePlugin.so  libSphereAtlasDemoPlugin.so
libArrangePlugin.so         libCessnaPlugin.so       libGimbalSmall2dPlugin.so   libJointTrajectoryPlugin.so  libLookAtDemoPlugin.so             libRegionEventBoxPlugin.so  libTimerGUIPlugin.so
libAttachLightPlugin.so     libContactPlugin.so      libGpuRayPlugin.so          libKeyboardGUIPlugin.so      libModelPropShop.so                libRestUiPlugin.so          libTouchPlugin.so
libBlinkVisualPlugin.so     libDepthCameraPlugin.so  libHarnessPlugin.so         libKeysToJointsPlugin.so     libMudPlugin.so                    libRestWebPlugin.so         libTransporterPlugin.so
libBreakableJointPlugin.so  libDiffDrivePlugin.so    libHeightmapLODPlugin.so    libLensFlareSensorPlugin.so  libPlaneDemoPlugin.so              libRubblePlugin.so          libVehiclePlugin.so
libBuoyancyPlugin.so        libElevatorPlugin.so     libHydraDemoPlugin.so       libLiftDragPlugin.so         libPressurePlugin.so               libSimEventsPlugin.so       libWindPlugin.so

kint@kint:~$ ls /usr/lib/x86_64-linux-gnu/gazebo-9/plugins  | grep libActorCollisionsPlugin

编译并拷贝好对应插件后
kint@kint:~$ ls /usr/lib/x86_64-linux-gnu/gazebo-9/plugins  | grep libActorCollisionsPlugin
libActorCollisionsPlugin.so

参考自己下载源码去编译，注意gazebo的版本和branch对应

附：在gazebo中添加碰撞插件

1.下载gazebo源码

git clone https://github.com/gazebosim/gazebo-classic 
注意branch: gazebo9

2.进入actor_collisions文件夹

cd examples/plugins/actor_collisions
3.编译碰撞插件

mkdir build
cd build
cmake ..
make
4.插件拷贝

sudo cp libActorCollisionsPlugin.so /usr/lib/x86_64-linux-gnu/gazebo-9/plugins 
5.测试插件

gazebo actor_collisions.world

actor_collisions.world 中的动态物体代码部分如下，如要在原静态world中添加，直接将actor标签整过去就行，修改相应的运行轨迹waypoint：

 <actor name="actor">

   <plugin name="actor_collisions_plugin" filename="libActorCollisionsPlugin.so">
     <scaling collision="LHipJoint_LeftUpLeg_collision" scale="
       0.01
       0.001
       0.001
     "/>
     <scaling collision="LeftUpLeg_LeftLeg_collision" scale="
       8.0
       8.0
       1.0
     "/>
     <scaling collision="LeftLeg_LeftFoot_collision" scale="
       8.0
       8.0
       1.0
     "/>
     <scaling collision="LeftFoot_LeftToeBase_collision" scale="
       4.0
       4.0
       1.5
     "/>
     <scaling collision="RHipJoint_RightUpLeg_collision" scale="
       0.01
       0.001
       0.001
     "/>
     <scaling collision="RightUpLeg_RightLeg_collision" scale="
       8.0
       8.0
       1.0
     "/>
     <scaling collision="RightLeg_RightFoot_collision" scale="
       8.0
       8.0
       1.0
     "/>
     <scaling collision="RightFoot_RightToeBase_collision" scale="
       4.0
       4.0
       1.5
     "/>
     <scaling collision="LowerBack_Spine_collision" scale="
       12.0
       20.0
       5.0
     " pose="0.05 0 0 0 -0.2 0"/>
     <scaling collision="Spine_Spine1_collision" scale="
       0.01
       0.001
       0.001
     "/>
     <scaling collision="Neck_Neck1_collision" scale="
       0.01
       0.001
       0.001
     "/>
     <scaling collision="Neck1_Head_collision" scale="
       5.0
       5.0
       3.0
     "/>
     <scaling collision="LeftShoulder_LeftArm_collision" scale="
       0.01
       0.001
       0.001
     "/>
     <scaling collision="LeftArm_LeftForeArm_collision" scale="
       5.0
       5.0
       1.0
     "/>
     <scaling collision="LeftForeArm_LeftHand_collision" scale="
       5.0
       5.0
       1.0
     "/>
     <scaling collision="LeftFingerBase_LeftHandIndex1_collision" scale="
       4.0
       4.0
       3.0
     "/>
     <scaling collision="RightShoulder_RightArm_collision" scale="
       0.01
       0.001
       0.001
     "/>
     <scaling collision="RightArm_RightForeArm_collision" scale="
       5.0
       5.0
       1.0
     "/>
     <scaling collision="RightForeArm_RightHand_collision" scale="
       5.0
       5.0
       1.0
     "/>
     <scaling collision="RightFingerBase_RightHandIndex1_collision" scale="
       4.0
       4.0
       3.0
     "/>
   </plugin>
   <skin>
     <filename>walk.dae</filename>
     <scale>1.0</scale>
   </skin>
   <animation name="walking">
     <filename>walk.dae</filename>
     <scale>1.000000</scale>
     <interpolate_x>true</interpolate_x>
   </animation>
   <script>
     <loop>true</loop>
     <delay_start>0.000000</delay_start>
     <auto_start>true</auto_start>
     <trajectory id="0" type="walking">
       <waypoint>
         <time>0.000000</time>
         <pose>0.000000 1.000000 0.000000 0.000000 0.000000 0.000000</pose>
       </waypoint>
       <waypoint>
         <time>0.500000</time>
         <pose>0.195090 0.980785 0.000000 0.000000 0.000000 -0.196350</pose>
       </waypoint>
       <waypoint>
          <time>1.000000</time>
          <pose>0.382683 0.923880 0.000000 0.000000 0.000000 -0.392699</pose>
       </waypoint>
       <waypoint>
          <time>1.500000</time>
          <pose>0.555570 0.831470 0.000000 0.000000 0.000000 -0.589049</pose>
       </waypoint>
       <waypoint>
          <time>2.000000</time>
          <pose>0.707107 0.707107 0.000000 0.000000 0.000000 -0.785398</pose>
       </waypoint>
       <waypoint>
          <time>2.500000</time>
          <pose>0.831470 0.555570 0.000000 0.000000 0.000000 -0.981748</pose>
       </waypoint>
       <waypoint>
          <time>3.000000</time>
          <pose>0.923880 0.382683 0.000000 0.000000 0.000000 -1.178100</pose>
       </waypoint>
       <waypoint>
          <time>3.500000</time>
          <pose>0.980785 0.195090 0.000000 0.000000 0.000000 -1.374450</pose>
       </waypoint>
       <waypoint>
          <time>4.000000</time>
          <pose>1.000000 0.000000 0.000000 0.000000 0.000000 -1.570800</pose>
       </waypoint>
       <waypoint>
          <time>4.500000</time>
          <pose>0.980785 -0.195090 0.000000 0.000000 0.000000 -1.767150</pose>
       </waypoint>
       <waypoint>
          <time>5.000000</time>
          <pose>0.923880 -0.382683 0.000000 0.000000 0.000000 -1.963500</pose>
       </waypoint>
       <waypoint>
          <time>5.500000</time>
          <pose>0.831470 -0.555570 0.000000 0.000000 0.000000 -2.159840</pose>
       </waypoint>
       <waypoint>
          <time>6.000000</time>
          <pose>0.707107 -0.707107 0.000000 0.000000 0.000000 -2.356190</pose>
       </waypoint>
       <waypoint>
          <time>6.500000</time>
          <pose>0.555570 -0.831470 0.000000 0.000000 0.000000 -2.552540</pose>
       </waypoint>
       <waypoint>
          <time>7.500000</time>
          <pose>0.382683 -0.923880 0.000000 0.000000 0.000000 -2.748890</pose>
       </waypoint>
       <waypoint>
          <time>8.500000</time>
          <pose>0.195090 -0.980785 0.000000 0.000000 0.000000 -2.945240</pose>
       </waypoint>
       <waypoint>
          <time>9.500000</time>
          <pose>0.000000 -1.000000 0.000000 0.000000 0.000000 -3.141590</pose>
       </waypoint>
       <waypoint>
          <time>10.500000</time>
          <pose>-0.195090 -0.980785 0.000000 0.000000 0.000000 2.945245</pose>
       </waypoint>
       <waypoint>
          <time>11.500000</time>
          <pose>-0.382683 -0.923880 0.000000 0.000000 0.000000 2.748895</pose>
       </waypoint>
       <waypoint>
          <time>12.000000</time>
          <pose>-0.555570 -0.831470 0.000000 0.000000 0.000000 2.552545</pose>
       </waypoint>
       <waypoint>
          <time>12.500000</time>
          <pose>-0.707107 -0.707107 0.000000 0.000000 0.000000 2.356195</pose>
       </waypoint>
       <waypoint>
          <time>13.000000</time>
          <pose>-0.831470 -0.555570 0.000000 0.000000 0.000000 2.159845</pose>
       </waypoint>
       <waypoint>
          <time>13.500000</time>
          <pose>-0.923880 -0.382683 0.000000 0.000000 0.000000 1.963495</pose>
       </waypoint>
       <waypoint>
          <time>14.000000</time>
          <pose>-0.980785 -0.195090 0.000000 0.000000 0.000000 1.767145</pose>
       </waypoint>
       <waypoint>
          <time>14.500000</time>
          <pose>-1.000000 0.000000 0.000000 0.000000 0.000000 1.570795</pose>
       </waypoint>
       <waypoint>
          <time>15.000000</time>
          <pose>-0.980785 0.195090 0.000000 0.000000 0.000000 1.374445</pose>
       </waypoint>
       <waypoint>
          <time>15.500000</time>
          <pose>-0.923880 0.382683 0.000000 0.000000 0.000000 1.178095</pose>
       </waypoint>
       <waypoint>
          <time>16.000000</time>
          <pose>-0.831470 0.555570 0.000000 0.000000 0.000000 0.981745</pose>
       </waypoint>
       <waypoint>
          <time>16.500000</time>
          <pose>-0.707107 0.707107 0.000000 0.000000 0.000000 0.785395</pose>
       </waypoint>
       <waypoint>
          <time>17.000000</time>
          <pose>-0.555570 0.831470 0.000000 0.000000 0.000000 0.589045</pose>
       </waypoint>
       <waypoint>
          <time>17.500000</time>
          <pose>-0.382683 0.923880 0.000000 0.000000 0.000000 0.392695</pose>
       </waypoint>
       <waypoint>
          <time>18.000000</time>
          <pose>-0.195090 0.980785 0.000000 0.000000 0.000000 0.196345</pose>
       </waypoint>
     </trajectory>
   </script>
 </actor>

激光可视效果图：

gazebo中动态障碍物实时pose

1. gazebo_ros_p3d 无法发布world中model的pose
   运动中机器人是可以通过这以odometer的形式发布话题
   https://github.com/ros-simulation/gazebo_ros_pkgs/blob/noetic-devel/gazebo_plugins/src/gazebo_ros_p3d.cpp

 <plugin name="robot_p3d" filename="libgazebo_ros_p3d.so">
   <alwaysOn>true</alwaysOn>
   <updateRate>50.0</updateRate>
   <bodyName>base_link</bodyName>
   <topicName>robot_pose</topicName>
   <gaussianNoise>0.01</gaussianNoise>
   <frameName>world</frameName>
   <xyzOffsets>0 0 0</xyzOffsets>
   <rpyOffsets>0 0 0</rpyOffsets>
 </plugin>   

动态模型使用gazebo_ros_p3d插件发布pose待研究

无效的
    <!-- <plugin name="people_p3d" filename="libgazebo_ros_p3d.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>50.0</updateRate>
      <bodyName>actor</bodyName>
      <topicName>people_pose</topicName>
      <gaussianNoise>0.01</gaussianNoise>
      <frameName>world</frameName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin> -->

2. gazebo提供相应的服务与话题接口可以查看model的pose信息

int@kint:~$ rostopic list | grep state
/gazebo/link_states
/gazebo/model_states

kint@kint:~$ rosservice list | grep state
/gazebo/get_link_state
/gazebo/get_model_state

kint@kint:~$ rostopic  echo  /gazebo/model_states
name: 
  - ground_plane
  - Untitled
  - cube_20k
  - Dumpster
  - Dumpster_0
  - jersey_barrier
  - actor
  - turtlebot3_burger
pose: 
  - 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  - 
    position: 
      x: 0.603431
      y: 0.0078
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  - 
    position: 
      x: -5.38602065236
      y: -6.80744881043
      z: 0.499999533763
    orientation: 
      x: -1.56147498596e-07
      y: 4.11218458777e-07
      z: 0.000350767461665
      w: 0.999999938481
  - 
    position: 
      x: 5.96634000329
      y: -7.4192400014
      z: 0.000917228865924
    orientation: 
      x: -4.4374537364e-08
      y: 8.20482662792e-07
      z: -3.21807825887e-11
      w: 1.0
  - 
    position: 
      x: 6.01622000329
      y: 6.8208899986
      z: 0.000917228865924
    orientation: 
      x: -4.4374537364e-08
      y: 8.20482662792e-07
      z: -3.21807825887e-11
      w: 1.0
  - 
    position: 
      x: -2.42989
      y: -0.195854
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.703329181425
      w: 0.710864306711
  - 
    position: 
      x: -0.762738254539
      y: -0.685481094471
      z: 1.032665
    orientation: 
      x: -0.224475849993
      y: 0.65300445035
      z: 0.665474749341
      w: -0.283441596422
  - 
    position: 
      x: -0.0451317038465
      y: 0.0244496431331
      z: -0.00100340529416
    orientation: 
      x: -0.00228018314614
      y: 0.00312514199549
      z: 0.588102512665
      w: 0.808777144119
twist: 
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  - 
    linear: 
      x: -0.00109069429249
      y: 0.000179601510918
      z: 1.84796141906e-05
    angular: 
      x: 7.65645076952e-05
      y: -1.82519279997e-05
      z: 0.0165613168548

kint@kint:~$ rosservice call /gazebo/get_model_state "model_name: 'actor'
relative_entity_name: ''" 
header: 
  seq: 1
  stamp: 
    secs: 628
    nsecs: 363000000
  frame_id: ''
pose: 
  position: 
    x: -0.734865579665
    y: 0.655232085776
    z: 1.030874
  orientation: 
    x: 0.261328317576
    y: 0.654221918987
    z: 0.66146012535
    w: 0.257238592986
twist: 
  linear: 
    x: 0.0
    y: 0.0
    z: 0.0
  angular: 
    x: 0.0
    y: 0.0
    z: 0.0
success: True
status_message: "GetModelState: got properties"

3. 常见一个ros节点程序，订阅gazebo模型发布的pose, 再将目标模型的pose以odometer方式发布