WebotsのROS2チュートリアルをC++に移植する

# はじめに
ROS2でいろいろやるにあたってGazebo代わりに使えるロボットシミュレータを探してるとWebotsというシミュレータがかなり良さげなので使って見ようと思ったんですが、ROS2のドキュメントにはPythonでのチュートリアルしかなかったのでC++で全く同じ動きをするパッケージを作ってみます。

https://docs.ros.org/en/galactic/Tutorials/Advanced/Simulators/Webots.html

# 対象者
ROS2のWebotsのチュートリアルを見ながらPythonで動かしてみたけどC++でもやってみたい人

# 動作環境
| 項目 | 値 |
|:-:|:-:|
| CPU | Core i7-1165G7 |
| Ubuntu | 20.04 |
| ROS | Galactic |

# 前準備
パッケージの作成
```shell
ros2 pkg create my_package_cpp --dependencies rclcpp geometry_msgs webots_ros2_driver pluginlib
```

本家のチュートリアルに倣って``worlds``フォルダを作成し``my_world.wbt``をダウンロードしてきて配置します

必要なファイルを作成します
- ``include/my_package_cpp/my_robot_driver.hpp``
- ``src/my_robot_driver.cpp``
- ``resource/my_robot.urdf``
- ``launch/robot_launcher.py``
- ``my_package_cpp.xml`` ← Pythonのときはなかった

# 本編
## my_robot_driver.hpp
``webots_ros2_driver::PluginInterface``を継承してプラグインを作成します

```c++
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <webots_ros2_driver/PluginInterface.hpp>
#include <webots_ros2_driver/WebotsNode.hpp>
#include <webots/Motor.hpp>
#include <webots/Robot.hpp>

namespace my_robot_driver
{

class MyRobotDriver : public webots_ros2_driver::PluginInterface {
public:
    MyRobotDriver() { }

    void init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters) override;
    void step() override;

    rclcpp::Logger get_logger()
    {
        return node_->get_logger();
    }

private:
    void cmd_vel_callback(geometry_msgs::msg::Twist::SharedPtr msg)
    {
        target_twist_ = msg;
    }

private:
    const double HALF_DISTANCE_BETWEEN_WHEELS = 0.045;
    const double WHEEL_RADIUS = 0.025;

    webots_ros2_driver::WebotsNode *node_; // rclcpp::Nodeを継承してる
    std::shared_ptr<webots::Motor> left_motor_;
    std::shared_ptr<webots::Motor> right_motor_;

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
    geometry_msgs::msg::Twist::SharedPtr target_twist_;
    
};

}
```

## my_robot_driver.cpp
モーターを速度制御したいときは位置の目標値に無限を設定しておかないと回転しないみたいです

```c++
#include "my_package_cpp/my_robot_driver.hpp"
#include "pluginlib/class_list_macros.hpp"

namespace my_robot_driver
{

void MyRobotDriver::init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &)
{
    node_ = node;
    RCLCPP_INFO(get_logger(), "MyRobotDriver::init");

    left_motor_ =  std::make_shared<webots::Motor>("left wheel motor");
    right_motor_ = std::make_shared<webots::Motor>("right wheel motor");

    left_motor_->setPosition(INFINITY); // モータを速度モードで動かしたいときは位置の目標値は無限にしておく
    left_motor_->setVelocity(0);

    right_motor_->setPosition(INFINITY); // モータを速度モードで動かしたいときは位置の目標値は無限にしておく
    right_motor_->setVelocity(0);
    
    target_twist_ = std::make_shared<geometry_msgs::msg::Twist>();
    twist_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", rclcpp::QoS(1),
        std::bind(&MyRobotDriver::cmd_vel_callback, this, std::placeholders::_1));
}

void MyRobotDriver::step()
{
    // RCLCPP_INFO(get_logger(), "MyRobotDriver::step");
    double forward_speed = target_twist_->linear.x;
    double angular_speed = target_twist_->angular.z;

    double command_motor_left = (forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS;
    double command_motor_right = (forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS;

    left_motor_->setVelocity(command_motor_left);
    right_motor_->setVelocity(command_motor_right);
}

} // namespace my_robot_driver

// ライブラリとしてexportするためのおまじない
PLUGINLIB_EXPORT_CLASS(my_robot_driver::MyRobotDriver, webots_ros2_driver::PluginInterface)
```

## package.xml
``webots_ros2_driver``と``pluginlib``が必要なので忘れずに入れましょう

```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_package_cpp</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="xxx@gmail.com">xxx</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>geometry_msgs</depend>
  <depend>webots_ros2_driver</depend> <!-- プラグイン用 -->
  <depend>pluginlib</depend> <!-- プラグイン用 -->
  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

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

## CMakeLists.txt
``ament_cmake_auto``を使ってできるだけ短くしました

https://hans-robo.hatenablog.com/entry/2020/12/15/153503

```cmake
cmake_minimum_required(VERSION 3.5)
project(my_package_cpp)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

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

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_library(${PROJECT_NAME} 
  SHARED
  src/my_robot_driver.cpp
)

# my_package_cpp.xmlをexportしないと認識されないので忘れずに
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
pluginlib_export_plugin_description_file(webots_ros2_driver my_package_cpp.xml)

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

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package()
```

## my_robot.urdf
``type``は``.*.cpp``の``PLUGINLIB_EXPORT_CLASS()``と``my_package_cpp.xml``と合わせます
```xml
<?xml version="1.0" ?>
<robot name="My robot">
    <webots>
        <!-- typeは "namespace::classname" -->
        <plugin type="my_robot_driver::MyRobotDriver" />
    </webots>
</robot>
```

## my_package_cpp.xml
このファイルがないとプラグインが認識されないので忘れずに作ります

```xml
<library path="my_package_cpp">
  <!-- typeは "namespace::classname" -->
  <class
    type="my_robot_driver::MyRobotDriver"
    base_class_type="webots_ros2_driver::PluginInterface">
    <description>
      turotial webots ros2 driver
    </description>
  </class>
</library>
```

## robot_launcher.py
``my_package``→``my_package_cpp``にしただけでほとんど変えてないです

```py
import os
import pathlib
import launch
from launch_ros.actions import Node
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.webots_launcher import WebotsLauncher, Ros2SupervisorLauncher
from webots_ros2_driver.utils import controller_url_prefix


def generate_launch_description():
    package_dir = get_package_share_directory('my_package_cpp')
    robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'my_robot.urdf')).read_text()

    webots = WebotsLauncher(
        world=os.path.join(package_dir, 'worlds', 'my_world.wbt')
    )

    ros2_supervisor = Ros2SupervisorLauncher()

    my_robot_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        output='screen',
        additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'my_robot'},
        parameters=[
            {'robot_description': robot_description},
        ]
    )

    return LaunchDescription([
        webots,
        my_robot_driver,
        ros2_supervisor,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        )
    ])
```

# 完成
ビルドが通ったら公式チュートリアルと同じように実行してWebotsが起動することを確認します
```shell
colcon build
source install/local_setup.bash
ros2 launch my_package robot_launch.py
```

ちゃんと起動したらターミナルを立ち上げてロボットを動かすためのコマンドを送ります
```shell
ros2 topic pub /cmd_vel geometry_msgs/Twist  "linear: { x: 0.1 }"
```

ロボットが前進したら成功です。お疲れ様でした。