1. はじめに

ロボットが突然壊れたらどんな挙動をするんだろうと、この前レストランでロボットが配膳してくるところを見て考えまして、いきなりロボットが吹っ飛ぶようなシミュレーションはできないかなと先日ふと思いました。。。。

調べていく中で、以下のリポジトリを見つけました。

内容を見てみると、まさにやりたいことに近いことができそうだったため、このリポジトリをベースに試してみることにしました。ただし、このプラグインは ROS1 向けに作られており、そのままでは使えません。

そこで本記事では、このプラグインを ROS2 環境で使えるように移植し、以下のことに取り組みました。

Gazebo Classic（gazebo11）上で、ロボットモデルを「爆破っぽく」吹き飛ばす
ROS2 のサービス /gazebo/destroy_model にモデル名を渡してロボットモデルを吹き飛ばす
ROS1 用プラグインを ROS2 Humbleで動作するように対応させてロボットモデルを吹き飛ばす

2. 実行環境

CPU: CORE i7 7th Gen
メモリ: 32GB
GPU: GeForce RTX 2070
OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)

3. 構築手順

3.1. 依存インストール

sudo apt update
sudo apt install -y \
  ros-humble-desktop \
  ros-humble-gazebo-ros-pkgs \
  python3-colcon-common-extensions \
  ros-humble-xacro

3.2. ROS2ワークスペースを作成してクローン

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/nlamprian/gazebo_model_destroyer_plugin.git

3.3. ROS2化の方針

元リポジトリは ROS1（catkin/roslaunch） 前提なので、以下を行います。

catkin → ament_cmake
roscpp → rclcpp
Gazeboプラグイン内のROSノード生成は gazebo_ros::Node::Get(sdf) を使う
.srv はROS2の rosidl_generate_interfaces() で生成
rosidl_generate_interfaces(${PROJECT_NAME} ...) と add_library(${PROJECT_NAME} ...) はターゲット名衝突するので プラグイン側ターゲット名を別名にする

3.4. `srv/Destroy.srv` を確認

リポジトリに srv/Destroy.srv がある前提（無い場合は作成する）。

Destroy.srv の内容（今回動いた定義）：

string model_name
---
bool success

3.5. `package.xml` をROS2用に修正（全文）

~/ros2_ws/src/gazebo_model_destroyer_plugin/package.xml を以下に置き換えました。

ポイント：gazebo_ros を <depend> と <exec_depend> の両方で書くとエラーになるので片方に統一しました

<package format="3">
  <name>gazebo_model_destroyer_plugin</name>
  <version>0.0.1</version>
  <description>Gazebo plugin to destroy models</description>

  <maintainer email="you@example.com">Your Name</maintainer>
  <license>BSD</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>gazebo_ros</depend>
  <depend>rclcpp</depend>
  <depend>ignition-math6</depend>
  <depend>gazebo_dev</depend>

  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

  <exec_depend>xacro</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

3.6. `CMakeLists.txt` をROS2用に修正（全文）

~/ros2_ws/src/gazebo_model_destroyer_plugin/CMakeLists.txt を以下に置き換えました。

ポイント：プラグインターゲット名を ${PROJECT_NAME}_plugin にして衝突回避
※ srvは srv/Destroy.srv を指定

cmake_minimum_required(VERSION 3.5)
project(gazebo_model_destroyer_plugin)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)

find_package(gazebo_dev REQUIRED)
find_package(gazebo_ros REQUIRED)
find_package(rclcpp REQUIRED)
find_package(ignition-math6 REQUIRED)

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/Destroy.srv"
)

set(PLUGIN_TARGET ${PROJECT_NAME}_plugin)

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

file(GLOB PLUGIN_SOURCES CONFIGURE_DEPENDS src/*.cpp)
add_library(${PLUGIN_TARGET} SHARED ${PLUGIN_SOURCES})

target_include_directories(${PLUGIN_TARGET} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  ${GAZEBO_INCLUDE_DIRS}
)

target_link_libraries(${PLUGIN_TARGET}
  ${GAZEBO_LIBRARIES}
  ignition-math6::ignition-math6
)

ament_target_dependencies(${PLUGIN_TARGET}
  gazebo_ros
  rclcpp
)

rosidl_get_typesupport_target(cpp_typesupport_target
  ${PROJECT_NAME}
  rosidl_typesupport_cpp
)
target_link_libraries(${PLUGIN_TARGET} "${cpp_typesupport_target}")

ament_export_dependencies(rosidl_default_runtime)

install(TARGETS ${PLUGIN_TARGET}
  LIBRARY DESTINATION lib
)

install(DIRECTORY include/
  DESTINATION include
)

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)
install(DIRECTORY test
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

3.7. プラグイン本体をROS2化

3.7.1 ヘッダ `gazebo_model_destroyer_plugin.h`（全文）

include/gazebo_model_destroyer_plugin/gazebo_model_destroyer_plugin.h を以下に置き換えました。

#ifndef GAZEBO_MODEL_DESTROYER_PLUGIN_H
#define GAZEBO_MODEL_DESTROYER_PLUGIN_H

#include <random>
#include <string>

#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <ignition/math/Vector3.hh>

#include <gazebo_ros/node.hpp>
#include <rclcpp/rclcpp.hpp>

#include "gazebo_model_destroyer_plugin/srv/destroy.hpp"

namespace gazebo {

class GazeboModelDestroyerPlugin : public WorldPlugin {
public:
  GazeboModelDestroyerPlugin();
  ~GazeboModelDestroyerPlugin() override;

  void Load(physics::WorldPtr world, sdf::ElementPtr sdf) override;

private:
  ignition::math::Vector3d getRandomVel(bool is_linear);

  void destroyServiceCallback(
      const std::shared_ptr<gazebo_model_destroyer_plugin::srv::Destroy::Request> request,
      std::shared_ptr<gazebo_model_destroyer_plugin::srv::Destroy::Response> response);

  physics::WorldPtr world_;

  gazebo_ros::Node::SharedPtr ros_node_;
  rclcpp::Service<gazebo_model_destroyer_plugin::srv::Destroy>::SharedPtr destroy_service_;

  std::random_device rd_;
  std::mt19937 gen_;
  std::uniform_real_distribution<> uni_dist_lin_;
  std::uniform_real_distribution<> uni_dist_ang_;
};

}  // namespace gazebo

#endif

3.7.2 実装 `gazebo_model_destroyer_plugin.cpp`（全文）

src/gazebo_model_destroyer_plugin.cpp を以下に置き換えました。

#include <gazebo_model_destroyer_plugin/gazebo_model_destroyer_plugin.h>
#include <functional>

namespace gazebo {

GazeboModelDestroyerPlugin::GazeboModelDestroyerPlugin()
    : gen_(rd_()) {}

GazeboModelDestroyerPlugin::~GazeboModelDestroyerPlugin() = default;

void GazeboModelDestroyerPlugin::Load(physics::WorldPtr world, sdf::ElementPtr sdf) {
  world_ = world;

  ros_node_ = gazebo_ros::Node::Get(sdf);

  if (!sdf->HasElement("linearVel")) {
    RCLCPP_ERROR(ros_node_->get_logger(), "Missing <linearVel> in SDF");
    return;
  }
  const double linear_vel = sdf->Get<double>("linearVel");
  uni_dist_lin_ = std::uniform_real_distribution<>(-linear_vel, linear_vel);

  if (!sdf->HasElement("angularVel")) {
    RCLCPP_ERROR(ros_node_->get_logger(), "Missing <angularVel> in SDF");
    return;
  }
  const double angular_vel = sdf->Get<double>("angularVel");
  uni_dist_ang_ = std::uniform_real_distribution<>(-angular_vel, angular_vel);

  if (!sdf->HasElement("destroyService")) {
    RCLCPP_ERROR(ros_node_->get_logger(), "Missing <destroyService> in SDF");
    return;
  }
  const std::string service_name = sdf->Get<std::string>("destroyService");

  destroy_service_ =
      ros_node_->create_service<gazebo_model_destroyer_plugin::srv::Destroy>(
          service_name,
          std::bind(&GazeboModelDestroyerPlugin::destroyServiceCallback,
                    this, std::placeholders::_1, std::placeholders::_2));

  RCLCPP_INFO(ros_node_->get_logger(), "Destroy service ready: %s", service_name.c_str());
}

ignition::math::Vector3d GazeboModelDestroyerPlugin::getRandomVel(bool is_linear) {
  ignition::math::Vector3d vel;
  if (is_linear) {
    vel.X() = uni_dist_lin_(gen_);
    vel.Y() = uni_dist_lin_(gen_);
    vel.Z() = uni_dist_lin_(gen_);
  } else {
    vel.X() = uni_dist_ang_(gen_);
    vel.Y() = uni_dist_ang_(gen_);
    vel.Z() = uni_dist_ang_(gen_);
  }
  return vel;
}

void GazeboModelDestroyerPlugin::destroyServiceCallback(
    const std::shared_ptr<gazebo_model_destroyer_plugin::srv::Destroy::Request> request,
    std::shared_ptr<gazebo_model_destroyer_plugin::srv::Destroy::Response> response) {

#if GAZEBO_MAJOR_VERSION > 7
  auto model = world_->ModelByName(request->model_name);
#else
  auto model = world_->GetModel(request->model_name);
#endif

  if (!model) {
    RCLCPP_WARN(ros_node_->get_logger(), "Model not found: %s", request->model_name.c_str());
    response->success = false;
    return;
  }

  for (auto & joint : model->GetJoints()) {
    joint->Detach();
  }

  for (auto & link : model->GetLinks()) {
    link->SetLinearVel(getRandomVel(true));
    link->SetAngularVel(getRandomVel(false));
  }

  response->success = true;
}

GZ_REGISTER_WORLD_PLUGIN(GazeboModelDestroyerPlugin)

}  // namespace gazebo

3.8. example.world を修正

元リポジトリの test/worlds/example.world はpluginの .so 名がROS1のままなので修正しました。

test/worlds/example.world（全文）

<sdf version='1.6'>
  <world name='default'>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <plugin name="gazebo_model_destroyer_plugin" filename="libgazebo_model_destroyer_plugin_plugin.so">
      <destroyService>/gazebo/destroy_model</destroyService>
      <linearVel>7</linearVel>
      <angularVel>3</angularVel>
    </plugin>

  </world>
</sdf>

3.9. ROS2用 launch を作成（Gazebo起動＋spawn）

launch/example_spawn_robot.launch.py を作成する。

ポイント：spawn_entity.py が使う /spawn_entity は GazeboRosFactory が必要
gzserver を -s libgazebo_ros_factory.so 付きで起動する。

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, TimerAction
from launch_ros.actions import Node

def generate_launch_description():
    pkg_share = get_package_share_directory('gazebo_model_destroyer_plugin')
    world_path = os.path.join(pkg_share, 'test', 'worlds', 'example.world')
    xacro_path = os.path.join(pkg_share, 'test', 'urdf', 'robot.xacro')
    urdf_out = '/tmp/robot.urdf'

    gzserver = ExecuteProcess(
        cmd=[
            'gzserver', '--verbose', world_path,
            '-s', 'libgazebo_ros_init.so',
            '-s', 'libgazebo_ros_factory.so'
        ],
        output='screen'
    )

    gzclient = ExecuteProcess(
        cmd=['gzclient'],
        output='screen'
    )

    gen_urdf = ExecuteProcess(
        cmd=['bash', '-lc', f'xacro \"{xacro_path}\" > \"{urdf_out}\"'],
        output='screen'
    )

    spawn_robot = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'robot', '-file', urdf_out, '-x', '0', '-y', '0', '-z', '0.5'],
        output='screen'
    )

    spawn_after_delay = TimerAction(
        period=5.0,
        actions=[gen_urdf, spawn_robot]
    )

    return LaunchDescription([gzserver, gzclient, spawn_after_delay])

3.10. ビルド

cd ~/ros2_ws
rm -rf build install log
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select gazebo_model_destroyer_plugin
source install/setup.bash

3.11. 実行（Gazebo起動 → robot spawn）

Gazeboがプラグインを見つけられるように GAZEBO_PLUGIN_PATH を設定しました。

terminal(1)

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:~/ros2_ws/install/gazebo_model_destroyer_plugin/lib

ros2 launch gazebo_model_destroyer_plugin example_spawn_robot.launch.py

3.11. モデル爆破（吹っ飛ばし）（別ターミナルでサービス呼び出し）

terminal(2)

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash

ros2 service call /gazebo/destroy_model \
  gazebo_model_destroyer_plugin/srv/Destroy \
  "{model_name: robot}"

以下が、モデル爆破（吹っ飛ばし）した時の動画です。

4.まとめ

Gazebo Classicでも「爆破っぽい演出（ジョイントを外して速度を与える）」は簡単に実現できました。今回の構成では、ROS2サービスでロボットモデル名を指定して、デモ的に「爆破（吹っ飛ばし）トリガ」を作れるのがポイントです。今後はこれをgazeboの新バージョンGZ Simでもできるか試してみようと思います。