cyberbotics · ygoumaz · Nov 18, 2022 · Oct 28, 2022 · Oct 28, 2022 · Oct 31, 2022
diff --git a/README.md b/README.md
@@ -12,7 +12,7 @@ It integrates with ROS2 using ROS2 messages, services and actions.
 
 Please visit the [documentation](https://github.com/cyberbotics/webots_ros2/wiki) that contains the following sections:
 - [Getting Started](https://github.com/cyberbotics/webots_ros2/wiki/Getting-Started)
-- [Build and Install](https://github.com/cyberbotics/webots_ros2/wiki/Build-and-Install)
+- [Complete Installation Guide](https://github.com/cyberbotics/webots_ros2/wiki/Complete-Installation-Guide)
 - [Tutorials](https://github.com/cyberbotics/webots_ros2/wiki/Tutorials)
 - [Examples](https://github.com/cyberbotics/webots_ros2/wiki/Examples)
 - [References](https://github.com/cyberbotics/webots_ros2/wiki/References)

diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package webots_ros2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2022.1.4 (2022-11-18)
+------------------
+* Fix the camera focal length in the CameraInfo topic.
+* Upgraded to urdf2webots 2.0.3
+* Update the calculation of CameraRecognitionObject messages to the RDF convention of R2022b.
+
+2022.1.3 (2022-11-02)
+------------------
+* Added macOS support.
+* Added reset handler to all examples to support simulation reset from Webots.
+
+2022.1.2 (2022-10-21)
+------------------
+* Added WSL support.
+
 2022.1.0 (2022-09-23)
 ------------------
 * Adapted controllers to communicate with Webots R2022b.

diff --git a/webots_ros2/package.xml b/webots_ros2/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>webots_ros2</name>
-  <version>2022.1.2</version>
+  <version>2022.1.4</version>
   <description>Interface between Webots and ROS2</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2/setup.py b/webots_ros2/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version='2022.1.2',
+    version='2022.1.4',
     packages=[package_name],
     data_files=[
         ('share/' + package_name, ['package.xml']),

diff --git a/webots_ros2_control/package.xml b/webots_ros2_control/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>webots_ros2_control</name>
-  <version>2022.1.2</version>
+  <version>2022.1.4</version>
   <description>ros2_control plugin for Webots</description>
   <maintainer email="[email protected]">Cyberbotics</maintainer>
   <url type="website">http://wiki.ros.org/webots_ros2</url>

diff --git a/webots_ros2_core/package.xml b/webots_ros2_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>webots_ros2_core</name>
-  <version>2022.1.2</version>
+  <version>2022.1.4</version>
   <description>Core interface between Webots and ROS2</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2_core/setup.py b/webots_ros2_core/setup.py
@@ -19,7 +19,7 @@
 
 setup(
     name=package_name,
-    version='2022.1.2',
+    version='2022.1.4',
     packages=[package_name, package_name + '.devices', package_name + '.math', package_name + '.webots'],
     data_files=data_files,
     install_requires=['setuptools', 'launch'],

diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst
@@ -2,11 +2,24 @@
 Changelog for package webots_ros2_driver
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2022.1.4 (2022-11-18)
+------------------
+* Fix the camera focal length in the CameraInfo topic.
+* Update the calculation of CameraRecognitionObject messages to the RDF convention of R2022b.
+
+2022.1.3 (2022-11-02)
+------------------
+* Added macOS support.
+
 2022.1.2 (2022-10-21)
 ------------------
 * Fix issue where relatively defined PROTO were not found.
 * Added WSL support.
 
+2022.1.1 (2022-10-11)
+------------------
+* Simplified the detection of Webots installation folder.
+
 2022.1.0 (2022-09-23)
 ------------------
 * Added an URDF importer feature to spawn robots from URDF files.

diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt
@@ -32,6 +32,7 @@ find_package(rclcpp REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(vision_msgs REQUIRED)
 find_package(webots_ros2_msgs REQUIRED)
@@ -137,6 +138,7 @@ ament_target_dependencies(driver
   pluginlib
   sensor_msgs
   std_msgs
+  tf2_geometry_msgs
   tf2_ros
   vision_msgs
   webots_ros2_msgs
@@ -258,6 +260,7 @@ ament_export_dependencies(
   rclpy
   sensor_msgs
   std_msgs
+  tf2_geometry_msgs
   tf2_ros
   vision_msgs
   webots_ros2_msgs

diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp
@@ -21,6 +21,8 @@
 
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>

diff --git a/webots_ros2_driver/package.xml b/webots_ros2_driver/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>webots_ros2_driver</name>
-  <version>2022.1.2</version>
+  <version>2022.1.4</version>
   <description>Implementation of the Webots - ROS 2 interface</description>
   <maintainer email="[email protected]">Cyberbotics</maintainer>
   <license>Apache License 2.0</license>
@@ -21,6 +21,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>vision_msgs</depend>
   <depend>tinyxml2_vendor</depend>

diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp
@@ -44,9 +44,7 @@ namespace webots_ros2_driver
     mCameraInfoMessage.distortion_model = "plumb_bob";
 
     // Convert FoV to focal length.
-    // Reference: https://en.wikipedia.org/wiki/Focal_length#In_photography
-    const double diagonal = sqrt(pow(mCamera->getWidth(), 2) + pow(mCamera->getHeight(), 2));
-    const double focalLength =  0.5 * diagonal * (cos(0.5 * mCamera->getFov()) / sin(0.5 * mCamera->getFov()));
+    const double focalLength = mCamera->getWidth() / (2 * tan(mCamera->getFov() / 2));
 
     mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0};
     mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
@@ -138,18 +136,25 @@ namespace webots_ros2_driver
     for (size_t i = 0; i < mCamera->getRecognitionNumberOfObjects(); i++)
     {
       // Getting Object Info
-      geometry_msgs::msg::Point position;
-      geometry_msgs::msg::Quaternion orientation;
-      position.x = objects[i].position[0];
-      position.y = objects[i].position[1];
-      position.z = objects[i].position[2];
-      axisAngleToQuaternion(objects[i].orientation, orientation);
+      geometry_msgs::msg::PoseStamped pose;
+      pose.pose.position.x = objects[i].position[0];
+      pose.pose.position.y = objects[i].position[1];
+      pose.pose.position.z = objects[i].position[2];
+      axisAngleToQuaternion(objects[i].orientation, pose.pose.orientation);
+
+      // Transform to ROS camera coordinate frame
+      // rpy = (0, pi/2, -pi/2)
+      geometry_msgs::msg::TransformStamped transform;
+      transform.transform.rotation.x = 0.5;
+      transform.transform.rotation.y = -0.5;
+      transform.transform.rotation.z = 0.5;
+      transform.transform.rotation.w = 0.5;
+      tf2::doTransform(pose, pose, transform);
 
       // Object Info -> Detection2D
       vision_msgs::msg::Detection2D detection;
       vision_msgs::msg::ObjectHypothesisWithPose hypothesis;
-      hypothesis.pose.pose.position = position;
-      hypothesis.pose.pose.orientation = orientation;
+      hypothesis.pose.pose = pose.pose;
       detection.results.push_back(hypothesis);
       #if FOXY || GALACTIC
       detection.bbox.center.x = objects[i].position_on_image[0];
@@ -166,8 +171,7 @@ namespace webots_ros2_driver
       webots_ros2_msgs::msg::CameraRecognitionObject recognitionWebotsObject;
       recognitionWebotsObject.id = objects[i].id;
       recognitionWebotsObject.model = std::string(objects[i].model);
-      recognitionWebotsObject.pose.pose.position = position;
-      recognitionWebotsObject.pose.pose.orientation = orientation;
+      recognitionWebotsObject.pose = pose;
       #if FOXY || GALACTIC
       recognitionWebotsObject.bbox.center.x = objects[i].position_on_image[0];
       recognitionWebotsObject.bbox.center.y = objects[i].position_on_image[1];

diff --git a/webots_ros2_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package webots_ros2_epuck
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2022.1.3 (2022-11-02)
+------------------
+* Added macOS support.
+* Added reset handler to support simulation reset from Webots.
+
 2022.1.2 (2022-10-21)
 ------------------
 * Added WSL support.

diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py
@@ -29,19 +29,11 @@
 from webots_ros2_driver.utils import controller_url_prefix
 
 
-def generate_launch_description():
+def get_ros2_nodes(*args):
     package_dir = get_package_share_directory('webots_ros2_epuck')
-    world = LaunchConfiguration('world')
     robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'epuck_webots.urdf')).read_text()
     ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml')
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
-
-    webots = WebotsLauncher(
-        world=PathJoinSubstitution([package_dir, 'worlds', world])
-    )
-
-    ros2_supervisor = Ros2SupervisorLauncher()
-
     controller_manager_timeout = ['--controller-manager-timeout', '50']
     controller_manager_prefix = 'python.exe' if os.name == 'nt' else ''
 
@@ -112,6 +104,35 @@ def generate_launch_description():
         arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
     )
 
+    return [
+        joint_state_broadcaster_spawner,
+        diffdrive_controller_spawner,
+        robot_state_publisher,
+        epuck_driver,
+        footprint_publisher,
+        epuck_process,
+    ]
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory('webots_ros2_epuck')
+    world = LaunchConfiguration('world')
+
+    webots = WebotsLauncher(
+        world=PathJoinSubstitution([package_dir, 'worlds', world])
+    )
+
+    ros2_supervisor = Ros2SupervisorLauncher()
+
+    # The following line is important!
+    # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends).
+    reset_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=ros2_supervisor,
+            on_exit=get_ros2_nodes,
+        )
+    )
+
     return LaunchDescription([
         DeclareLaunchArgument(
             'world',
@@ -120,18 +141,20 @@ def generate_launch_description():
         ),
         webots,
         ros2_supervisor,
-        joint_state_broadcaster_spawner,
-        diffdrive_controller_spawner,
-        robot_state_publisher,
-        epuck_driver,
-        footprint_publisher,
-        epuck_process,
 
         # This action will kill all nodes once the Webots simulation has exited
         launch.actions.RegisterEventHandler(
             event_handler=launch.event_handlers.OnProcessExit(
                 target_action=webots,
-                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
+                on_exit=[
+                    launch.actions.UnregisterEventHandler(
+                        event_handler=reset_handler.event_handler
+                    ),
+                    launch.actions.EmitEvent(event=launch.events.Shutdown())
+                ],
             )
-        )
-    ])
+        ),
+
+        # Add the reset event handler
+        reset_handler
+    ] + get_ros2_nodes())
diff --git a/webots_ros2_epuck/package.xml b/webots_ros2_epuck/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>webots_ros2_epuck</name>
-  <version>2022.1.2</version>
+  <version>2022.1.4</version>
   <description>E-puck2 driver for Webots simulated robot</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2_epuck/setup.py b/webots_ros2_epuck/setup.py
@@ -43,7 +43,7 @@
 
 setup(
     name=package_name,
-    version='2022.1.2',
+    version='2022.1.4',
     packages=[package_name],
     data_files=data_files,
     install_requires=['setuptools', 'launch'],

diff --git a/webots_ros2_importer/CHANGELOG.rst b/webots_ros2_importer/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package webots_ros2_importer
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2022.1.4 (2022-11-18)
+------------------
+* Upgraded to urdf2webots 2.0.3
+
 2022.1.1 (2022-10-11)
 ------------------
 * Upgraded to urdf2webots 2.0.2

diff --git a/webots_ros2_importer/package.xml b/webots_ros2_importer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>webots_ros2_importer</name>
-  <version>2022.1.2</version>
+  <version>2022.1.4</version>
   <description>This package allows to convert URDF and XACRO files into Webots PROTO files.</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2_importer/setup.py b/webots_ros2_importer/setup.py
@@ -10,7 +10,7 @@
 
 setup(
     name=package_name,
-    version='2022.1.2',
+    version='2022.1.4',
     packages=[package_name, package_name + '.urdf2webots.urdf2webots'],
     data_files=data_files,
     install_requires=[