Merge develop into master

scripts/ci_before_init_embed.bash

@@ -2,7 +2,7 @@
 
 ROS_DISTRO=$1
 
-export WEBOTS_RELEASE_VERSION=2023a-rev1
+export WEBOTS_RELEASE_VERSION=2023b
 export WEBOTS_OFFSCREEN=1
 export CI=1
 export DEBIAN_FRONTEND=noninteractive

webots_ros2/CHANGELOG.rst

@@ -6,12 +6,19 @@ Changelog for package webots_ros2
 ------------------
 * Added support for Navigation2 in Iron.
 * Clean simulation reset in launch files.
+* Added new TIAGo project to webots_ros2_tiago to run real robot configuration.
+* Added new WebotsController node in the driver interface to launch robot controller plugins.
+* Fixed unfound robot window library in Tesla example.
+* Default to canonical topic name and fix camera_info stamp in Ros2Camera, Ros2RangeFinder.
+* Added VacuumGripper gripper support in webots_ros2_driver.
+* Added BoolStamped message in webots_ros2_msgs.
+* Added GetBool message in webots_ros2_msgs.
 
 2023.0.4 (2023-05-23)
 ------------------
 * Drop support for Foxy.
 * Fixed vertical field of view in static RangeFinder plugin.
-* Added support for painted point clouds
+* Added support for painted point clouds.
 * Fixed ability to launch RViz without other tools in e-puck example.
 * Fixed command line arguments in importer tools.
 * Added custom handler in driver interface to start nodes when Webots is ready.

webots_ros2_driver/CHANGELOG.rst

@@ -2,10 +2,18 @@
 Changelog for package webots_ros2_driver
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2023.1.0 (2023-XX-XX)
+------------------
+* Added parameters to rename Camera and RangeFinder topics.
+* Added new WebotsController node to launch robot controller plugins.
+* Added compilation of the generic robot window library.
+* Default to canonical topic name and fix camera_info stamp in Ros2Camera, Ros2RangeFinder.
+* Added VacuumGripper device support
+
 2023.0.4 (2023-05-23)
 ------------------
 * Fixed vertical field of view in static RangeFinder plugin.
-* Added support for painted point clouds
+* Added support for painted point clouds.
 * Added custom handler to start nodes when Webots is ready.
 
 2023.0.3 (2023-04-12)

webots_ros2_driver/CMakeLists.txt

@@ -41,6 +41,12 @@ add_custom_target(compile-lib-vehicle ALL
   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/webots/projects/default/libraries/vehicle
 )
 
+add_custom_target(compile-generic-window ALL
+  COMMAND ${CMAKE_COMMAND} -E env "WEBOTS_HOME=${CMAKE_CURRENT_SOURCE_DIR}/webots" make release -f Makefile > /dev/null 2>&1
+  DEPENDS compile-lib-controller
+  WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/webots/resources/projects/libraries/generic_robot_window
+)
+
 set(WEBOTS_LIB_BASE webots/lib/controller)
 
 include_directories(
@@ -71,7 +77,6 @@ ament_python_install_package(${PROJECT_NAME}
   PACKAGE_DIR ${PROJECT_NAME})
 
 # Driver
-set(CMAKE_INSTALL_RPATH "$ORIGIN/../controller")
 add_executable(driver
   src/Driver.cpp
   src/WebotsNode.cpp
@@ -87,6 +92,7 @@ add_executable(driver
   src/plugins/static/Ros2Emitter.cpp
   src/plugins/static/Ros2Receiver.cpp
   src/plugins/static/Ros2Compass.cpp
+  src/plugins/static/Ros2VacuumGripper.cpp
   src/utils/Math.cpp
   src/utils/Utils.cpp
 )
@@ -119,7 +125,6 @@ install(TARGETS driver
 )
 
 # Dynamic IMU
-set(CMAKE_INSTALL_RPATH "$ORIGIN/controller")
 add_library(
   ${PROJECT_NAME}_imu
   SHARED
@@ -147,7 +152,6 @@ install(TARGETS ${PROJECT_NAME}_imu
 )
 
 # Dynamic RGBD
-set(CMAKE_INSTALL_RPATH "$ORIGIN/controller")
 add_library(
   ${PROJECT_NAME}_rgbd
   SHARED
@@ -193,6 +197,7 @@ install(
   PATTERN "*CppCar*"
   PATTERN "*driver*"
   PATTERN "*CppDriver*"
+  PATTERN "*generic_robot_window*"
 )
 
 # Prevent pluginlib from using boost
@@ -213,6 +218,12 @@ install(
   DESTINATION share/${PROJECT_NAME}/scripts
 )
 
+# Install webots-controller script
+install(
+  PROGRAMS webots/webots-controller
+  DESTINATION share/${PROJECT_NAME}/scripts
+)
+
 # Ament export
 set(WEBOTS_LIB_PATH
   controller/${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX}
@@ -221,6 +232,7 @@ set(WEBOTS_LIB_PATH
   controller/${CMAKE_SHARED_LIBRARY_PREFIX}CppDriver${CMAKE_SHARED_LIBRARY_SUFFIX}
   controller/${CMAKE_SHARED_LIBRARY_PREFIX}car${CMAKE_SHARED_LIBRARY_SUFFIX}
   controller/${CMAKE_SHARED_LIBRARY_PREFIX}CppCar${CMAKE_SHARED_LIBRARY_SUFFIX}
+  controller/${CMAKE_SHARED_LIBRARY_PREFIX}generic_robot_window${CMAKE_SHARED_LIBRARY_SUFFIX}
 )
 
 ament_export_include_directories(

webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp

@@ -47,6 +47,7 @@ namespace webots_ros2_driver {
     void setAnotherNodeParameter(std::string anotherNodeName, std::string parameterName, std::string parameterValue);
     rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr mClient;
 
+    std::string mRobotDescriptionParam;
     std::string mRobotDescription;
     bool mSetRobotStatePublisher;
 

webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp

@@ -50,6 +50,9 @@ namespace webots_ros2_driver {
 
     WbDeviceTag mCamera;
 
+    std::string mCameraInfoSuffix;
+    std::string mImageSuffix;
+
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mImagePublisher;
     sensor_msgs::msg::Image mImageMessage;
 

webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp

@@ -36,6 +36,10 @@ namespace webots_ros2_driver {
 
     WbDeviceTag mRangeFinder;
 
+    std::string mCameraInfoSuffix;
+    std::string mImageSuffix;
+    std::string mPointCloudSuffix;
+
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mImagePublisher;
     sensor_msgs::msg::Image mImageMessage;
     rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr mCameraInfoPublisher;

webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2VacuumGripper.hpp

@@ -0,0 +1,56 @@
+// Copyright 1996-2023 Cyberbotics Ltd.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS2_VACUUM_GRIPPER_HPP
+#define ROS2_VACUUM_GRIPPER_HPP
+
+#include <webots_ros2_driver/WebotsNode.hpp>
+#include <webots_ros2_driver/plugins/Ros2SensorPlugin.hpp>
+
+#include <std_msgs/msg/bool.hpp>
+#include <webots_ros2_msgs/msg/bool_stamped.hpp>
+#include <webots_ros2_msgs/srv/get_bool.hpp>
+
+namespace webots_ros2_driver {
+  class Ros2VacuumGripper : public Ros2SensorPlugin {
+  public:
+    void step() override;
+    void init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters) override;
+
+  private:
+    void publishPresence();
+
+    // ROS2 subscriptions
+    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr mTurnOnSubscription;
+    void turnOnCallback(const std_msgs::msg::Bool::SharedPtr message);
+
+    // ROS2 services
+    rclcpp::Service<webots_ros2_msgs::srv::GetBool>::SharedPtr mIsOnService;
+    void isOnCallback(const std::shared_ptr<webots_ros2_msgs::srv::GetBool::Request> request,
+                      std::shared_ptr<webots_ros2_msgs::srv::GetBool::Response> response);
+
+    // ROS2 topics
+    rclcpp::Publisher<webots_ros2_msgs::msg::BoolStamped>::SharedPtr mPresencePublisher;
+    webots_ros2_msgs::msg::BoolStamped mPresenceMessage;
+    bool mIsPresenceEnabled;
+
+    // Device
+    WbDeviceTag mVacuumGripper;
+    // Runtime vars
+    std::string mDeviceName;
+  };
+
+}  // namespace webots_ros2_driver
+
+#endif

webots_ros2_driver/src/PythonPlugin.cpp

@@ -29,11 +29,6 @@ namespace webots_ros2_driver {
 import os
 import sys
 
-# Set WEBOTS_HOME to the webots_ros2_driver installation folder
-# to load the correct libController libraries from the Python API
-from ament_index_python.packages import get_package_prefix
-os.environ['WEBOTS_HOME'] = get_package_prefix('webots_ros2_driver')
-
 import controller
 from controller import Supervisor
 

webots_ros2_driver/src/WebotsNode.cpp

@@ -31,6 +31,7 @@
 #include <webots_ros2_driver/plugins/static/Ros2LightSensor.hpp>
 #include <webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp>
 #include <webots_ros2_driver/plugins/static/Ros2Receiver.hpp>
+#include <webots_ros2_driver/plugins/static/Ros2VacuumGripper.hpp>
 #include "webots_ros2_driver/PluginInterface.hpp"
 
 #include "webots_ros2_driver/PluginInterface.hpp"
@@ -49,13 +50,45 @@ namespace webots_ros2_driver {
   }
 
   WebotsNode::WebotsNode(std::string name) : Node(name), mPluginLoader(gPluginInterfaceName, gPluginInterface) {
-    mRobotDescription = this->declare_parameter<std::string>("robot_description", "");
+    mRobotDescriptionParam = this->declare_parameter<std::string>("robot_description", "");
     mSetRobotStatePublisher = this->declare_parameter<bool>("set_robot_state_publisher", false);
-    if (mRobotDescription != "") {
+    if (mRobotDescriptionParam != "") {
       mRobotDescriptionDocument = std::make_shared<tinyxml2::XMLDocument>();
-      mRobotDescriptionDocument->Parse(mRobotDescription.c_str());
+      // Path to URDF file
+      if (mRobotDescriptionParam.rfind(".urdf") == mRobotDescriptionParam.size() - 5)
+        mRobotDescriptionDocument->LoadFile(mRobotDescriptionParam.c_str());
+      // Path to Xacro file
+      else if (mRobotDescriptionParam.rfind(".xacro") == mRobotDescriptionParam.size() - 6) {
+        std::vector<std::string> xacroMappings =
+          this->declare_parameter<std::vector<std::string>>("xacro_mappings", std::vector<std::string>());
+        std::string command = "ros2 run xacro xacro " + mRobotDescriptionParam;
+        for (const std::string &xacroMapping : xacroMappings) {
+          command += " ";
+          command += xacroMapping;
+        }
+
+        FILE *stream = popen(command.c_str(), "r");
+        if (!stream)
+          throw std::runtime_error("Failed to execute xacro command");
+
+        char buffer[4096];
+        std::string xacroOutput;
+        while (fgets(buffer, sizeof(buffer), stream) != NULL)
+          xacroOutput += buffer;
+        pclose(stream);
+
+        mRobotDescriptionDocument->Parse(xacroOutput.c_str());
+      }
+      // Full string (deprecated)
+      else {
+        mRobotDescriptionDocument->Parse(mRobotDescriptionParam.c_str());
+        RCLCPP_WARN(
+          get_logger(),
+          "\033[33mPassing robot description as a string is deprecated. Provide the URDF or Xacro file path instead.\033[0m");
+      }
       if (!mRobotDescriptionDocument)
-        throw std::runtime_error("Invalid URDF, it cannot be parsed");
+        throw std::runtime_error("Invalid robot description, it cannot be parsed");
+      // Access the robot and webots elements as needed
       tinyxml2::XMLElement *robotXMLElement = mRobotDescriptionDocument->FirstChildElement("robot");
       if (!robotXMLElement)
         throw std::runtime_error("Invalid URDF, it doesn't contain a <robot> tag");
@@ -64,6 +97,10 @@ namespace webots_ros2_driver {
       mWebotsXMLElement = NULL;
       RCLCPP_INFO(get_logger(), "Robot description is not passed, using default parameters.");
     }
+    // Store robot description string in mRobotDescription
+    tinyxml2::XMLPrinter printer;
+    mRobotDescriptionDocument->Accept(&printer);
+    mRobotDescription = printer.CStr();
 
     mRemoveUrdfRobotPublisher = create_publisher<std_msgs::msg::String>("/remove_urdf_robot", rclcpp::ServicesQoS());
     mRemoveUrdfRobotMessage.data = name;
@@ -165,6 +202,9 @@ namespace webots_ros2_driver {
         case WB_NODE_COMPASS:
           plugin = std::make_shared<webots_ros2_driver::Ros2Compass>();
           break;
+        case WB_NODE_VACUUM_GRIPPER:
+          plugin = std::make_shared<webots_ros2_driver::Ros2VacuumGripper>();
+          break;
       }
       if (plugin) {
         plugin->init(this, parameters);

webots_ros2_driver/src/plugins/static/Ros2Camera.cpp

@@ -23,10 +23,14 @@ namespace webots_ros2_driver {
     mRecognitionIsEnabled = false;
     mCamera = wb_robot_get_device(parameters["name"].c_str());
 
+    mCameraInfoSuffix = parameters.count("cameraInfoSuffix") ? parameters["cameraInfoSuffix"] : "/camera_info";
+    mImageSuffix = parameters.count("imageSuffix") ? parameters["imageSuffix"] : "/image_color";
+
     assert(mCamera != 0);
 
     // Image publisher
-    mImagePublisher = mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName, rclcpp::SensorDataQoS().reliable());
+    mImagePublisher =
+      mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName + mImageSuffix, rclcpp::SensorDataQoS().reliable());
     mImageMessage.header.frame_id = mFrameName;
     mImageMessage.height = wb_camera_get_height(mCamera);
     mImageMessage.width = wb_camera_get_width(mCamera);
@@ -37,7 +41,7 @@ namespace webots_ros2_driver {
 
     // CameraInfo publisher
     mCameraInfoPublisher =
-      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable());
+      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + mCameraInfoSuffix, rclcpp::SensorDataQoS().reliable());
     mCameraInfoMessage.header.stamp = mNode->get_clock()->now();
     mCameraInfoMessage.header.frame_id = mFrameName;
     mCameraInfoMessage.height = wb_camera_get_height(mCamera);
@@ -116,6 +120,7 @@ namespace webots_ros2_driver {
     auto image = wb_camera_get_image(mCamera);
     if (image) {
       mImageMessage.header.stamp = mNode->get_clock()->now();
+      mCameraInfoMessage.header.stamp = mImageMessage.header.stamp;
       memcpy(mImageMessage.data.data(), image, mImageMessage.data.size());
       mImagePublisher->publish(mImageMessage);
     }

webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp

@@ -25,13 +25,18 @@ namespace webots_ros2_driver {
     mIsEnabled = false;
     mRangeFinder = wb_robot_get_device(parameters["name"].c_str());
 
+    mCameraInfoSuffix = parameters.count("cameraInfoSuffix") ? parameters["cameraInfoSuffix"] : "/camera_info";
+    mImageSuffix = parameters.count("imageSuffix") ? parameters["imageSuffix"] : "/image";
+    mPointCloudSuffix = parameters.count("pointCloudSuffix") ? parameters["pointCloudSuffix"] : "/point_cloud";
+
     assert(mRangeFinder != 0);
 
     const int width = wb_range_finder_get_width(mRangeFinder);
     const int height = wb_range_finder_get_height(mRangeFinder);
 
     // Image publisher
-    mImagePublisher = mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName, rclcpp::SensorDataQoS().reliable());
+    mImagePublisher =
+      mNode->create_publisher<sensor_msgs::msg::Image>(mTopicName + mImageSuffix, rclcpp::SensorDataQoS().reliable());
     mImageMessage.header.frame_id = mFrameName;
     mImageMessage.height = height;
     mImageMessage.width = width;
@@ -42,7 +47,7 @@ namespace webots_ros2_driver {
 
     // CameraInfo publisher
     mCameraInfoPublisher =
-      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable());
+      mNode->create_publisher<sensor_msgs::msg::CameraInfo>(mTopicName + mCameraInfoSuffix, rclcpp::SensorDataQoS().reliable());
     mCameraInfoMessage.header.stamp = mNode->get_clock()->now();
     mCameraInfoMessage.header.frame_id = mFrameName;
     mCameraInfoMessage.height = height;
@@ -59,8 +64,8 @@ namespace webots_ros2_driver {
                             1.0,          0.0};
 
     // Point cloud publisher
-    mPointCloudPublisher =
-      mNode->create_publisher<sensor_msgs::msg::PointCloud2>(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable());
+    mPointCloudPublisher = mNode->create_publisher<sensor_msgs::msg::PointCloud2>(mTopicName + mPointCloudSuffix,
+                                                                                  rclcpp::SensorDataQoS().reliable());
     mPointCloudMessage.header.frame_id = mFrameName;
     mPointCloudMessage.fields.resize(3);
     mPointCloudMessage.fields[0].name = "x";
@@ -118,6 +123,7 @@ namespace webots_ros2_driver {
     auto image = wb_range_finder_get_range_image(mRangeFinder);
     if (image) {
       mImageMessage.header.stamp = mNode->get_clock()->now();
+      mCameraInfoMessage.header.stamp = mImageMessage.header.stamp;
       memcpy(mImageMessage.data.data(), image, mImageMessage.data.size());
       mImagePublisher->publish(mImageMessage);
     }