Tutorial Creating a Custom Cpp Plugin - cyberbotics/webots

Since webots_ros2 1.1.0 the package includes the webots_ros2_driver. The main objectives of the sub-package are:

It automatically creates a ROS 2 interface out of a Webots robot model.
It allows users to configure the ROS 2 interface in URDF files.
It allows users to extend the interface using the pluginlib plugin mechanism.

In this tutorial, we will explain how to create a custom C++ plugin that utilizes the webots_ros2_driver plugin mechanism.

See the Creating a Custom Python Plugin tutorial for Python plugins.

`webots_ros2_driver` Architecture

Please take a look at the picture below to get a better idea of the plugin system architecture.

webots_ros2_driver architecture

Plugin File Structure

The tree below depicts the minimal file structure for a custom plugin.

ros2_ws
└── src    
    └── webots_ros2_plugin_example
        ├── include
        │   └── webots_ros2_plugin_example
        │       └── WebotsRos2PluginExample.hpp
        ├── src
        │   └── WebotsRos2PluginExample.cpp
        ├── webots_ros2_plugin_example.xml
        ├── CMakeLists.txt
        └── package.xml

The include/webots_ros2_plugin_example/WebotsRos2PluginExample.hpp file, see the Plugin C++ Header File section.
The src/WebotsRos2PluginExample file, see the Plugin C++ Implementation File section.
The webots_ros2_plugin_example.xml pluginlib description file (see the pluginlib Description File section).
The CMakeLists.txt file (see the Plugin CMake File section).
The package.xml file (see the package.xml section).

Plugin Files

This section shows the critical files used for creating a Webots plugin package (and pluginlib packages in general).

Plugin C++ Header File

#ifndef WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
#define WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP

#include "rclcpp/macros.hpp"
#include "webots_ros2_driver/PluginInterface.hpp"
#include "webots_ros2_driver/WebotsNode.hpp"

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"

namespace webots_ros2_plugin_example
{
  class WebotsRos2PluginExample : public webots_ros2_driver::PluginInterface
  {
  public:
    // Your plugin has to override step() and init() methods
    void step() override;
    void init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters) override;

  private:
    bool preStep();
    void publishData();
    void cmd_vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);

    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_subscription_;
    geometry_msgs::msg::TwistStamped cmd_vel_msg;

    // Motors
    WbDeviceTag right_motor;
    WbDeviceTag left_motor;

    double publish_timestep;
    bool mAlwaysOn;
    int mPublishTimestepSyncedMs;
    double last_update;

  };
}
#endif

Plugin C++ Implementation File

#include "webots_ros2_plugin_example/WebotsRos2PluginExample.hpp"

#include "rclcpp/rclcpp.hpp"
#include <webots/motor.h>
#include <webots/robot.h>
#include <functional>
#include <cstdio>

using std::placeholders::_1;

#define HALF_DISTANCE_BETWEEN_WHEELS 0.08
#define WHEEL_RADIUS 0.033

namespace webots_ros2_plugin_example
{
  void WebotsRos2PluginExample::init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters)
  {
    // This method is executed once the plugin is loaded by the `webots_ros2_driver` package.
    // The `webots_ros2_driver::WebotsNode` inherits the `rclcpp::Node`, so you have all methods available from there.
    // The C API of the Webots controller library must be used.
    
    // Print a simple message to see if your plugin has been loaded correctly:
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Hello from WebotsRos2PluginExample!");
    right_motor = wb_robot_get_device("right wheel motor");
    left_motor = wb_robot_get_device("left wheel motor");
  
    // This parameter is read when loading the URDF file
    publish_timestep = parameters.count("updateRate") ? 1.0 / atof(parameters["updateRate"].c_str()) : 0;

    // Initialize motors
    wb_motor_set_position(right_motor, INFINITY);
    wb_motor_set_position(left_motor, INFINITY);
    wb_motor_set_velocity(right_motor, 0.0);
    wb_motor_set_velocity(left_motor, 0.0);
    
    cmd_vel_subscription_ = node->create_subscription<geometry_msgs::msg::TwistStamped>("/cmd_vel_plugin",
                                                                                       rclcpp::SensorDataQoS().reliable(),
                                                                                       std::bind(&WebotsRos2PluginExample::cmd_vel_callback, 
                                                                                       this, _1));

  }
  void WebotsRos2PluginExample::step()
  {
    // This method is executed on each Webots step
    auto forward_speed = cmd_vel_msg.twist.linear.x;
    auto angular_speed = cmd_vel_msg.twist.angular.z;

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

    wb_motor_set_velocity(left_motor, command_motor_left);
    wb_motor_set_velocity(right_motor, command_motor_right);
  }

  bool WebotsRos2PluginExample::preStep() {
    // Update only if needed
    if (wb_robot_get_time() - last_update < publish_timestep)
      return false;
    last_update = wb_robot_get_time();
    return true;
  }

  void WebotsRos2PluginExample::cmd_vel_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg){
    cmd_vel_msg.header = msg->header;
    cmd_vel_msg.twist = msg->twist;
  }

}

// The class has to be exported with `PLUGINLIB_EXPORT_CLASS` macro.
// The first argument is the name of your class, while the second is always `webots_ros2_driver::PluginInterface`
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(webots_ros2_plugin_example::WebotsRos2PluginExample, webots_ros2_driver::PluginInterface)

`pluginlib` Description File

This file xml is necessary for the pluginlib to be able to find your Webots ROS 2 plugin. For this example, the file is "webots_ros2_plugin_example.xml", which is later reference in the CMakeList.txt file using the pluginlib_export_plugin_description_file macro.

<library path="webots_ros2_plugin_example">
  <!-- The `type` attribute is a reference to the plugin class. -->
  <!-- The `base_class_type` attribute is always `webots_ros2_driver::PluginInterface`. -->
  <class type="webots_ros2_plugin_example::WebotsRos2PluginExample" base_class_type="webots_ros2_driver::PluginInterface">
    <description>
      This is a Webots ROS 2 plugin example
    </description>
  </class>
</library>

Plugin CMake File

The CMake file slightly differs from typical ROS 2 CMake files as it includes Webots header files and pluginlib.

cmake_minimum_required(VERSION 3.5)
project(webots_ros2_plugin_example)

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

# Besides the package specific dependencies we also need the `pluginlib` and `webots_ros2_driver`
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(webots_ros2_driver REQUIRED)

# Export the plugin configuration file
pluginlib_export_plugin_description_file(webots_ros2_driver webots_ros2_plugin_example.xml)

# The rest is standard ROS 2 packaging description
add_library(
  ${PROJECT_NAME}
  SHARED
  src/WebotsRos2PluginExample.cpp
)
target_include_directories(
  ${PROJECT_NAME}
  PRIVATE
  include
)
ament_target_dependencies(
  ${PROJECT_NAME}
  pluginlib
  rclcpp
  webots_ros2_driver
)
install(TARGETS
  ${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)
ament_export_include_directories(
  include
)
ament_export_libraries(
  ${PROJECT_NAME}
)
ament_package()

Package.xml

package.xml is a file containing meta information about the package.

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
  <name>webots_ros2_plugin_example</name>
  <version>0.0.0</version>
  <description>Plugin example in C++.</description>

  <maintainer email="[email protected]">Example</maintainer>
  <license>TODO: License declaration</license>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>webots_ros2_driver</exec_depend>
  <exec_depend>pluginlib</exec_depend>

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

Use the plugin in your package

You can create another custom package with your Webots project. Instuctions to do so can be found in the official ROS 2 documentation.

In the robot description file (e.g. my_robot.urdf), under the <webots> tag, you should include the plugin as following:

<?xml version="1.0" ?>
<robot name="My robot">
    <webots>
        <plugin type="webots_ros2_plugin_example::WebotsRos2PluginExample">
            <updateRate>100</updateRate>
        </plugin>
    </webots>
</robot>

Check if your plugin is running properly

When loading your robot in Webots using its URDF, you should see the message "Hello from WebotsRos2PluginExample!". Also, if you list the available ros2 topics (ros2 topic list), you should see the new topic /cmd_vel_plugin.

You can move your robot around with the following command:

ros2 topic pub /cmd_vel_plugin geometry_msgs/TwistStamped "twist: { linear: { x: 0.1 } }"

Tutorial Creating a Custom Cpp Plugin - cyberbotics/webots_ros2 GitHub Wiki

`webots_ros2_driver` Architecture

Plugin File Structure

Plugin Files

Plugin C++ Header File

Plugin C++ Implementation File

`pluginlib` Description File

Plugin CMake File

Package.xml

Use the plugin in your package

Check if your plugin is running properly

⚠️ GitHub.com Fallback ⚠️

Tutorial Creating a Custom Cpp Plugin - cyberbotics/webots_ros2 GitHub Wiki

webots_ros2_driver Architecture

Plugin File Structure

Plugin Files

Plugin C++ Header File

Plugin C++ Implementation File

pluginlib Description File

Plugin CMake File

Package.xml

Use the plugin in your package

Check if your plugin is running properly

⚠️ **GitHub.com Fallback** ⚠️

`webots_ros2_driver` Architecture

`pluginlib` Description File

⚠️ GitHub.com Fallback ⚠️