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> -->
  1. 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"


  1. 常见一个ros节点程序,订阅gazebo模型发布的pose, 再将目标模型的pose以odometer方式发布
### 如何在Gazebo仿真环境中实现动态障碍物 #### 创建自定义模型文件 为了实现在Gazebo中生成动态障碍物,通常先要创建一个自定义的静态或动态模型。这可以通过编写SDF(Simulation Description Format)文件完成。该文件描述了物体的几何形状、物理属性以及材料特性等信息[^1]。 ```xml <?xml version="1.0"?> <sdf version="1.6"> <model name='dynamic_obstacle'> <!-- 定义链接 --> <link name='body'> <pose>0 0 0.5 0 0 0</pose> <!-- 添加碰撞体 --> <collision name='main_collision'> <geometry><box><size>0.3 0.3 1</size></geometry> </collision> <!-- 可视化部分 --> <visual name='main_visual'> <geometry><box><size>0.3 0.3 1</size></geometry> </visual> <!-- 设置惯性参数 --> <inertial> <mass>1.0</mass> <inertia> <ixx>0.083</ixx> <iyy>0.083</iyy> <izz>0.083</izz> </inertia> </inertial> <!-- 插件用于控制移动行为 --> <plugin filename="libgazebo_ros_diff_drive.so" name="ros_diff_drive_plugin"/> </link> </model> </sdf> ``` #### 使用`spawn_entity.py`脚本加载模型 一旦有了上述SDF格式的模型文件,就可以通过Python脚本来调用ROS的服务接口,在指定位置实例化这些对象。下面是一个简单的例子: ```python import rospy from gazebo_msgs.srv import SpawnModel from geometry_msgs.msg import Pose rospy.init_node('spawn_model') initial_pose = Pose() initial_pose.position.x = 2.0 initial_pose.position.y = 3.0 initial_pose.position.z = 0.5 f = open("/path/to/your/model_file.sdf",'r') sdff = f.read() rospy.wait_for_service('/gazebo/spawn_sdf_model') spawn_model_prox = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) spawn_model_prox("obstacle_name", sdff, "", initial_pose, "world") print ("Spawn model get success!") ``` 这段代码会读取本地存储的一个SDF文件,并将其作为新的实体放置到世界坐标系中的特定位置。 #### 控制动态障碍物运动 为了让障碍物能够按照预设轨迹或其他逻辑方式移动起来,则需要借助插件机制或者额外开发节点来发送速度命令给已有的差分驱动控制器。对于更复杂的路径规划需求来说,还可以考虑集成导航堆栈或者其他高级算法库来进行实时计算并更新目标姿态[^3]。 例如可以使用如下C++代码片段向已经装备好diff-drive plugin 的模型发布线性和角速度指令: ```cpp #include <ros/ros.h> #include <geometry_msgs/Twist.h> int main(int argc, char **argv){ ros::init(argc, argv, "move_dynamic_obstacle"); ros::NodeHandle nh; ros::Publisher pub_cmd_vel = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); while(ros::ok()){ geometry_msgs::Twist msg; // 设定前进方向的速度值 msg.linear.x = 0.5; // 如果希望原地旋转则设置角速度 msg.angular.z = M_PI / 4; pub_cmd_vel.publish(msg); sleep(1); // 模拟持续一段时间的动作 } } ``` 以上方法展示了如何利用ROSGazebo的强大组合快速建立具有交互性的虚拟测试场景,这对于验证自主导航策略尤其有用处[^2]。
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值