Gazebo上に歩行者を障害物として出現させる！

1月 9, 2023

ROSのシミュレーション上では動かないテーブルや壁を障害物として出現させることは標準搭載のworldで実装されていますが, 予期しない動きをしない障害物なので少々物足りないです. 現実世界では予期しない動く障害物があり, 例えばその代表例に人間があると思います. 今回はロボットシミュレーションの中にそのロボットの進行を妨げる人間を出現させる方法を紹介します！

歩行者モデル

$$ F_{total} = F_{goal} + \sum F_{obs} + \sum F_{soc} $$

歩行者の動きのモデルとしてはSocial Force Modelになっています. このモデルでは歩行者が進む向きや力(黄色矢印)がその目的地に向かう引力(青矢印), 他の歩行者や障害物から受ける斥力(青矢印)の合算で決まってきます.

セットアップ

上記で説明した歩行者の動きを決めるSocial Force Modelのコードを取得し, ビルドします.

git clone https://github.com/robotics-upo/lightsfm.git
cd lightsfm
make
sudo make install

上記のコードをgazebo上で使用できるようにするプラグインをビルドします.

mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/tech-life-hacking/sfm_for_gazebo.git
cd ..
rosdep install -i --from-paths src
catkin build

うまくビルドができればlaunchファイルを立ち上げてみましょう. カフェ内を歩く人間を追加できていることを確認できていると思います.

source devel/setup.bash
roslaunch sfm_for_gazebo sfm.launch

各自のロボットの障害物として歩行者を追加する際のポイント

次は上記のような歩行者を各自のロボットの障害物として出現させましょう. 主に実施することは下記です.

  • worldファイルに歩行者の記載
  • 歩行者の動作を決める上記でビルドしたSocial Force Modelを動作させるためのプラグインの追加

下記のようなフォルダ構成にしています. 青色でハッチングしているファイルを追加し, 橙色でハッチングしたファイルに追記する必要があります.

  • catkin_ws
    • src
      • YOURPACKAGE
        • include
          • gazebo_sfm_plugin
            • PedestrianSFMRandomPlugin.h
        • launch
          • YOURFILE.launch
        • models
          • walk.dae
        • src
          • AttachModelPlugin.cc
          • AttachModelPlugin.hh
          • PedestrianSFMRandomPlugin.cpp
          • initial_random.cc
        • worlds
          • YOURFILE.world
        • CMakeLists.txt
        • package.xml

各自使用しているROSパッケージに歩行者モデルを組み込む際に気を付ける点は下記の4つのポイントです.

  1. 歩行者の素材の配置
  2. Gazebo上において, 上記でビルドしたSocial Force Modelを動作させるためのプラグインの追加
  3. worldに歩行者を追加
  4. プラグインをビルド対象にするためにCMakeLists.txtとpackage.xmlの編集

1. 歩行者の素材

フォルダ構成にあるようにcatkin_ws/src/YOURPACKAGE/models直下にdae形式の歩行者の素材を置きます. またworldファイルを呼び出すcatkin_ws/src/YOURFILE.launchファイルに下記の記載を追加します.

  <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find YOURPACKAGE)/models"/>
  <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find YOURPACKAGE)/models"/>

2. Gazebo上で動作させるためのプラグインの追加

フォルダ構成にあるようにcatkin_ws/src/YOURPACKAGE/src直下にGazebo上で動作させるためのプラグインを追加します.

3. worldに歩行者を追加

catkin_ws/src/YOURPACKAGE/worlds/YOURFILE.worldファイルに下記の記載を追加します. 下記リストは歩行者の動作を決めるパラメータです.

  • velocity : 歩行者の最大速度(m/s)
  • radius : 歩行者の体の半径(m)
  • people_distance : 周囲の歩行者の最大検出距離
  • ignore_obstacles : 障害物として無視するモデル. World内の他のアクターは自動的に含まれる.
  • trajectory : アクターが到達しなければならないチェックポイントのリスト
  • waypoint : チェックポイント(座標X, Y, Z)
  • cyclic : trueの場合, アクターは最後のチェックポイントに到達した後, 最初のチェックポイントから繰り返す
<model name="actor1_collision_model">
      <pose>0 0 0 0 0 0</pose>
      <static>true</static>
      <link name="link">
        <collision name="link">
          <pose>0 -0.18 0.00 1.57 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.3</radius>
              <length>1.8</length>
            </cylinder>
          </geometry>
        </collision>
        <!-- <visual name="link">
          <pose>0 -0.18 0.00 1.57 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.3</radius>
              <length>1.8</length>
            </cylinder>
          </geometry>
        </visual> -->
      </link>
    </model>

    <actor name="actor1">
      <pose>7 0 -1 0 0 0</pose>
      <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>

      <plugin name="actor1_plugin" filename="libPedestrianSFMRandomPlugin.so">
        <velocity>1.00</velocity>
        <radius>0.4</radius>
        <animation_factor>5.1</animation_factor>
        <people_distance>6.0</people_distance>
        <!-- weights -->
        <goal_weight>2.0</goal_weight>
        <obstacle_weight>0.0</obstacle_weight> <!--10.0-->
        <social_weight>15</social_weight> <!--2.1-->
        <group_gaze_weight>3.0</group_gaze_weight>
        <group_coh_weight>2.0</group_coh_weight>
        <group_rep_weight>1.0</group_rep_weight>
        <ignore_obstacles>
          <model>ground_plane</model>
          <model>actor1_collision_model</model>
        </ignore_obstacles>
        <trajectory>
          <cyclic>true</cyclic>
          <waypoint>3 0 -1</waypoint>
          <waypoint>-3 0 -1</waypoint>
        </trajectory>
        <!-- <random_trajectory>
          <steps>10</steps> -->
          <!-- <seed>6723872</seed> -->
          <!-- <origin>10.0 0.0 0.0</origin>
          <rx>11.0</rx><ry>6.0</ry>
        </random_trajectory> -->
      </plugin>

      <plugin name="attach_model1" filename="libAttachModelPlugin.so">
        <link>
          <link_name>actor1_pose</link_name>
          <model>
            <model_name>actor1_collision_model</model_name>
          </model>
        </link>
      </plugin>
    </actor>

4. プラグインをビルド対象にするためにCMakeLists.txtとpackage.xmlの編集

catkin_ws/src/CMakeLists.txtに下記の記載を追加します. 

#############################
# Plugins
#############################

find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})

find_package(Protobuf REQUIRED)
find_package(gazebo REQUIRED)

set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})

add_library(initial_random SHARED src/initial_random.cc)
target_link_libraries(initial_random ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})

# SFM random
find_package(catkin REQUIRED COMPONENTS
  gazebo_ros
  roscpp
)

find_package(Boost REQUIRED COMPONENTS thread)
find_package(gazebo REQUIRED)

include_directories(include)
include_directories(SYSTEM
  /usr/local/include  #to find lightsfm
  ${Boost_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
  ${GAZEBO_INCLUDE_DIRS}
)

link_directories(
  ${catkin_LIBRARY_DIRS}
  ${GAZEBO_LIBRARY_DIRS}
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES PedestrianSFMPlugin
  CATKIN_DEPENDS gazebo_ros roscpp
)

add_library(PedestrianSFMRandomPlugin src/PedestrianSFMRandomPlugin.cpp)
target_link_libraries(PedestrianSFMRandomPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) #${Boost_LIBRARIES

install(TARGETS
  PedestrianSFMRandomPlugin
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

# #############################
# ## Attach Model plugin ##
# #############################

# Create the libAttachModelPlugin.so library.
set(attach_model_plugin_name AttachModelPlugin)
add_library(${attach_model_plugin_name} SHARED
  src/AttachModelPlugin.cc
)
target_link_libraries(${attach_model_plugin_name}
  ${GAZEBO_LIBRARIES}
  ${roscpp_LIBRARIES}
)
#add_dependencies(${attach_model_plugin_name}
#  ${PROJECT_NAME}_generate_messages_cpp
#)
install(TARGETS ${attach_model_plugin_name}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

catkin_ws/src/package.xmlに下記の記載を追加

  <build_depend>lightsfm</build_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_export_depend>lightsfm</build_export_depend>
  <build_export_depend>gazebo_ros</build_export_depend>
  <exec_depend>lightsfm</exec_depend>
  <exec_depend>gazebo_ros</exec_depend>

  <export>
    <!-- Other tools can request additional information be placed here -->
    <gazebo_ros gazebo_media_path="${prefix}"/>
    <gazebo_ros gazebo_model_path="${prefix}/models"/>
    <gazebo_ros plugin_path="${prefix}/../../lib"/>
  </export>

4つのポイントを押さえたら, ビルドしてlaunchしましょう. 下図のように歩行者が現れればうまくいっています. 歩行者が現れない場合, その初期位置がカメラからかなり遠い位置にあることがあるので, Zoom outして歩行者がいないか確認してみた方がいいです.

よくあるエラー

下記エラーが生じる際は

    from scripts import GazeboRosPaths
ModuleNotFoundError: No module named 'scripts'

gazebo_rosがインストールできていないことがあります.

sudo apt install ros-noetic-gazebo-ros-pkgs

おわりに

いかがだったでしょうか. 動的な障害物である歩行者を追加することで, 予期しない失敗モードを洗い出すことができるかもしれないので, ぜひお試しください！

ROSgazebo,ROS

Posted by Tech Life Hacking