Merge branch 'develop' into sync-master-b901918

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

@@ -5,12 +5,16 @@ Changelog for package webots_ros2
 2023.1.0 (2023-XX-XX)
 ------------------
 * Added support for Navigation2 in Iron.
+* 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.
 
 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,17 @@
 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.
+
 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
@@ -119,7 +124,6 @@ install(TARGETS driver
 )
 
 # Dynamic IMU
-set(CMAKE_INSTALL_RPATH "$ORIGIN/controller")
 add_library(
   ${PROJECT_NAME}_imu
   SHARED
@@ -147,7 +151,6 @@ install(TARGETS ${PROJECT_NAME}_imu
 )
 
 # Dynamic RGBD
-set(CMAKE_INSTALL_RPATH "$ORIGIN/controller")
 add_library(
   ${PROJECT_NAME}_rgbd
   SHARED
@@ -193,6 +196,7 @@ install(
   PATTERN "*CppCar*"
   PATTERN "*driver*"
   PATTERN "*CppDriver*"
+  PATTERN "*generic_robot_window*"
 )
 
 # Prevent pluginlib from using boost
@@ -213,6 +217,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 +231,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/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

@@ -49,13 +49,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 +96,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;

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);
     }

webots_ros2_driver/webots/.gitignore

@@ -65,6 +65,7 @@ Thumbs.db
 /.clang-format
 /msys64
 /webots
+/webots-controller
 /webots.lnk
 /webots_debug_output.txt
 /util

webots_ros2_driver/webots/include/controller/c/webots/nodes.h

@@ -49,6 +49,7 @@ typedef enum {
   WB_NODE_PLANE,
   WB_NODE_POINT_LIGHT,
   WB_NODE_POINT_SET,
+  WB_NODE_POSE,
   WB_NODE_SHAPE,
   WB_NODE_SPHERE,
   WB_NODE_SPOT_LIGHT,

webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp

@@ -57,6 +57,7 @@ namespace webots {
       PLANE,
       POINT_LIGHT,
       POINT_SET,
+      POSE,
       SHAPE,
       SPHERE,
       SPOT_LIGHT,

webots_ros2_driver/webots/resources/version.txt

@@ -1 +1 @@
-R2023a revision 1
+R2023b

webots_ros2_driver/webots/src/controller/Makefile

@@ -29,6 +29,11 @@ release debug profile clean:
 	@echo "# Java controller library ("$@")"
 	@echo "#"
 	@+make -s -C java $@
+	@echo "#"	
 	@echo "# Matlab controller library ("$@")"
 	@echo "#"
 	@+make -s -C matlab $@
+	@echo "#"	
+	@echo "# Controller launcher ("$@")"
+	@echo "#"
+	@+make -s -C launcher $@

webots_ros2_driver/webots/src/controller/c/Controller.def

@@ -74,6 +74,7 @@ wb_NODE_PBR_APPEARANCE
 wb_NODE_PLANE
 wb_NODE_POINT_LIGHT
 wb_NODE_POINT_SET
+wb_NODE_POSE
 wb_NODE_SHAPE
 wb_NODE_SPHERE
 wb_NODE_SPOT_LIGHT

webots_ros2_driver/webots/src/controller/c/Makefile

@@ -36,7 +36,13 @@ include $(WEBOTS_HOME_PATH)/resources/Makefile.include
 WEBOTS_DEPENDENCY_PATH ?= $(WEBOTS_HOME_PATH)/dependencies
 
 ifeq ($(OSTYPE),darwin)
-CFLAGS      = -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -fpascal-strings -Wno-deprecated-declarations -I/Developer/Headers/FlatCarbon -Wall -fno-common
+LIBCONTROLLER_VERSION := "$(shell cat $(WEBOTS_HOME_PATH)/Contents/Resources/version.txt)"
+else
+LIBCONTROLLER_VERSION := "$(shell cat $(WEBOTS_HOME_PATH)/resources/version.txt)"
+endif
+
+ifeq ($(OSTYPE),darwin)
+CFLAGS      = -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -fpascal-strings -Wno-deprecated-declarations -I/Developer/Headers/FlatCarbon -Wall -fno-common -D LIBCONTROLLER_VERSION='$(LIBCONTROLLER_VERSION)'
 INCLUDE     = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include
 LD_FLAGS    = -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -dynamiclib -install_name @rpath/Contents/lib/controller/libController.dylib -compatibility_version 1.0 -current_version 1.0.0
 SHARED_LIBS = -lz
@@ -45,15 +51,15 @@ CC          = gcc
 endif
 
 ifeq ($(OSTYPE),linux)
-CFLAGS      = -fPIC -Wall -Wno-stringop-truncation
+CFLAGS      = -fPIC -Wall -Wno-stringop-truncation -D LIBCONTROLLER_VERSION='$(LIBCONTROLLER_VERSION)'
 INCLUDE     = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include
 LD_FLAGS    = -shared -Wl,-soname,libController.so
 SHARED_LIBS = -lm -lpthread -ldl -lrt
 TARGET      = $(WEBOTS_CONTROLLER_LIB_PATH)/libController.so
 endif
 
 ifeq ($(OSTYPE),windows)
-CFLAGS      = -Wall -mwindows -frandom-seed=0
+CFLAGS      = -Wall -mwindows -frandom-seed=0 -D LIBCONTROLLER_VERSION='$(LIBCONTROLLER_VERSION)'
 INCLUDE     = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include -I/mingw64/include
 LD_FLAGS    = -shared -mwindows --def Controller.def -lws2_32
 PLATFORM    = win32

webots_ros2_driver/webots/src/controller/c/node.c

@@ -28,9 +28,10 @@ const int wb_NODE_APPEARANCE = WB_NODE_APPEARANCE, wb_NODE_BACKGROUND = WB_NODE_
           wb_NODE_INDEXED_LINE_SET = WB_NODE_INDEXED_LINE_SET, wb_NODE_MATERIAL = WB_NODE_MATERIAL, wb_NODE_MESH = WB_NODE_MESH,
           wb_NODE_MUSCLE = WB_NODE_MUSCLE, wb_NODE_NORMAL = WB_NODE_NORMAL, wb_NODE_PBR_APPEARANCE = WB_NODE_PBR_APPEARANCE,
           wb_NODE_PLANE = WB_NODE_PLANE, wb_NODE_POINT_LIGHT = WB_NODE_POINT_LIGHT, wb_NODE_POINT_SET = WB_NODE_POINT_SET,
-          wb_NODE_SHAPE = WB_NODE_SHAPE, wb_NODE_SPHERE = WB_NODE_SPHERE, wb_NODE_SPOT_LIGHT = WB_NODE_SPOT_LIGHT,
-          wb_NODE_TEXTURE_COORDINATE = WB_NODE_TEXTURE_COORDINATE, wb_NODE_TEXTURE_TRANSFORM = WB_NODE_TEXTURE_TRANSFORM,
-          wb_NODE_TRANSFORM = WB_NODE_TRANSFORM, wb_NODE_VIEWPOINT = WB_NODE_VIEWPOINT;
+          wb_NODE_POSE = WB_NODE_POSE, wb_NODE_SHAPE = WB_NODE_SHAPE, wb_NODE_SPHERE = WB_NODE_SPHERE,
+          wb_NODE_SPOT_LIGHT = WB_NODE_SPOT_LIGHT, wb_NODE_TEXTURE_COORDINATE = WB_NODE_TEXTURE_COORDINATE,
+          wb_NODE_TEXTURE_TRANSFORM = WB_NODE_TEXTURE_TRANSFORM, wb_NODE_TRANSFORM = WB_NODE_TRANSFORM,
+          wb_NODE_VIEWPOINT = WB_NODE_VIEWPOINT;
 // robots
 int wb_NODE_ROBOT = WB_NODE_ROBOT;
 // devices
@@ -178,6 +179,8 @@ const char *wb_node_get_name(WbNodeType t) {
       return "PointSet";
     case WB_NODE_POSITION_SENSOR:
       return "PositionSensor";
+    case WB_NODE_POSE:
+      return "Pose";
     case WB_NODE_PROPELLER:
       return "Propeller";
     case WB_NODE_RADAR: