LoginSignup
1
1

More than 1 year has passed since last update.

@Kotakku

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

Last updated at Posted at 2023-01-22

はじめに

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

対象者

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

動作環境

項目 値
CPU Core i7-1165G7
Ubuntu 20.04
ROS Galactic

前準備

パッケージの作成

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を継承してプラグインを作成します

#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

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

#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 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を使ってできるだけ短くしました

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 version="1.0" ?>
<robot name="My robot">
    <webots>
        <!-- typeは "namespace::classname" -->
        <plugin type="my_robot_driver::MyRobotDriver" />
    </webots>
</robot>

my_package_cpp.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にしただけでほとんど変えてないです

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が起動することを確認します

colcon build
source install/local_setup.bash
ros2 launch my_package robot_launch.py

ちゃんと起動したらターミナルを立ち上げてロボットを動かすためのコマンドを送ります

ros2 topic pub /cmd_vel geometry_msgs/Twist  "linear: { x: 0.1 }"

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

1
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1