Adding Connector Plugin

webots_ros2_driver/CMakeLists.txt

@@ -105,6 +105,7 @@ add_executable(driver
   src/plugins/static/Ros2Receiver.cpp
   src/plugins/static/Ros2Compass.cpp
   src/plugins/static/Ros2VacuumGripper.cpp
+  src/plugins/static/Ros2Connector.cpp
   src/utils/Math.cpp
   src/utils/Utils.cpp
 )

webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Connector.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_CONNECTOR_HPP
+#define ROS2_CONNECTOR_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/int_stamped.hpp>
+#include <webots_ros2_msgs/srv/get_bool.hpp>
+
+namespace webots_ros2_driver {
+  class Ros2Connector : 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 mLockSubscription;
+    void LockCallback(const std_msgs::msg::Bool::SharedPtr message);
+
+    // ROS2 services
+    rclcpp::Service<webots_ros2_msgs::srv::GetBool>::SharedPtr mLockService;
+    void isLockedCallback(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::IntStamped>::SharedPtr mPresencePublisher;
+    webots_ros2_msgs::msg::IntStamped mPresenceMessage;
+    bool mIsPresenceEnabled;
+
+    // Device
+    WbDeviceTag mConnector;
+    // Runtime vars
+    std::string mDeviceName;
+  };
+
+}  // namespace webots_ros2_driver
+
+#endif

webots_ros2_driver/src/WebotsNode.cpp

@@ -25,6 +25,7 @@
 
 #include <webots_ros2_driver/plugins/static/Ros2Camera.hpp>
 #include <webots_ros2_driver/plugins/static/Ros2Compass.hpp>
+#include <webots_ros2_driver/plugins/static/Ros2Connector.hpp>
 #include <webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp>
 #include <webots_ros2_driver/plugins/static/Ros2Emitter.hpp>
 #include <webots_ros2_driver/plugins/static/Ros2GPS.hpp>
@@ -261,6 +262,9 @@ namespace webots_ros2_driver {
         case WB_NODE_VACUUM_GRIPPER:
           plugin = std::make_shared<webots_ros2_driver::Ros2VacuumGripper>();
           break;
+        case WB_NODE_CONNECTOR:
+          plugin = std::make_shared<webots_ros2_driver::Ros2Connector>();
+          break;
       }
       if (plugin) {
         plugin->init(this, parameters);

webots_ros2_driver/src/plugins/static/Ros2Connector.cpp

@@ -0,0 +1,83 @@
+// 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.
+
+#include "webots_ros2_driver/plugins/static/Ros2Connector.hpp"
+
+#include <webots/connector.h>
+#include <webots/robot.h>
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+
+namespace webots_ros2_driver {
+  void Ros2Connector::init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters) {
+    Ros2SensorPlugin::init(node, parameters);
+
+    // This parameter is read when loading the URDF file
+    mDeviceName = parameters.count("name") ? parameters["name"] : "connector";
+    mConnector = wb_robot_get_device(mDeviceName.c_str());
+
+    assert(mConnector != 0);
+
+    // Initialize services, publishers and subcriptions
+    mLockSubscription = mNode->create_subscription<std_msgs::msg::Bool>(
+      mTopicName + "/lock", rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2Connector::LockCallback, this, _1));
+
+    mLockService = mNode->create_service<webots_ros2_msgs::srv::GetBool>(
+      mTopicName + "/is_locked", std::bind(&Ros2Connector::isLockedCallback, this, _1, _2));
+
+    mIsPresenceEnabled = false;
+    mPresencePublisher =
+      mNode->create_publisher<webots_ros2_msgs::msg::IntStamped>(mTopicName + "/presence", rclcpp::SensorDataQoS().reliable());
+  }
+
+  void Ros2Connector::LockCallback(const std_msgs::msg::Bool::SharedPtr message) {
+    if (message->data)
+      wb_connector_lock(mConnector);
+    else
+      wb_connector_unlock(mConnector);
+  }
+
+  void Ros2Connector::isLockedCallback(const std::shared_ptr<webots_ros2_msgs::srv::GetBool::Request> request,
+                                       std::shared_ptr<webots_ros2_msgs::srv::GetBool::Response> response) {
+    response->value = wb_connector_is_locked(mConnector);
+  }
+
+  void Ros2Connector::step() {
+    if (!preStep())
+      return;
+
+    if (mIsPresenceEnabled)
+      publishPresence();
+
+    if (mAlwaysOn)
+      return;
+
+    // Enable/Disable sensor
+    const bool shouldBeEnabled = mPresencePublisher->get_subscription_count() > 0;
+    if (shouldBeEnabled != mIsPresenceEnabled) {
+      if (shouldBeEnabled)
+        wb_connector_enable_presence(mConnector, mPublishTimestepSyncedMs);
+      else
+        wb_connector_disable_presence(mConnector);
+      mIsPresenceEnabled = shouldBeEnabled;
+    }
+  }
+
+  void Ros2Connector::publishPresence() {
+    mPresenceMessage.header.stamp = mNode->get_clock()->now();
+    mPresenceMessage.data = wb_connector_get_presence(mConnector);
+    mPresencePublisher->publish(mPresenceMessage);
+  }
+}  // namespace webots_ros2_driver

webots_ros2_msgs/CMakeLists.txt

@@ -26,6 +26,7 @@ set(msg_files
   "msg/BoolStamped.msg"
   "msg/FloatStamped.msg"
   "msg/StringStamped.msg"
+  "msg/IntStamped.msg"
   "msg/CameraRecognitionObject.msg"
   "msg/CameraRecognitionObjects.msg"
   "msg/UrdfRobot.msg"

webots_ros2_msgs/msg/IntStamped.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+int8 data

webots_ros2_tests/resource/connector_and_vacuum_gripper_test.urdf

@@ -0,0 +1,22 @@
+<?xml version="1.0" ?>
+<robot name="connector_and_vacuum_gripper_test_robot_webots">
+
+    <webots>
+        <plugin type="webots_ros2_control::Ros2Control" />
+    </webots>
+
+    <ros2_control name="robot_control" type="system">
+        <hardware>
+            <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
+        </hardware>
+        <joint name="connector_motor">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="vacuum_gripper_motor">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+    </ros2_control>
+
+</robot>

webots_ros2_tests/resource/connector_and_vacuum_gripper_test_ros2_control.yml

@@ -0,0 +1,29 @@
+/robot/controller_manager:
+  ros__parameters:
+    update_rate: 50
+
+    connector_and_vacuum_gripper_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+/robot/connector_and_vacuum_gripper_trajectory_controller:
+  ros__parameters:
+    joints:
+      - connector_motor
+      - vacuum_gripper_motor
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+    action_monitor_rate: 20.0
+
+    allow_partial_joints_goal: false
+    interpolate_from_desired_state: true
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0

webots_ros2_tests/resource/object_position.urdf.xacro

@@ -0,0 +1,22 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="object">
+    <xacro:arg name="object_name" default="generic" />
+    <webots>
+        <device reference="$(arg object_name)_position" type="GPS">
+            <ros>
+                <enabled>true</enabled>
+                <alwaysOn>true</alwaysOn>
+            </ros>
+        </device>
+        <plugin type="webots_ros2_control::Ros2Control" />
+    </webots>
+
+    <ros2_control name="$(arg object_name)_control" type="system">
+        <hardware>
+            <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
+        </hardware>
+    </ros2_control>
+
+    <link name=""/>
+
+</robot>

webots_ros2_tests/setup.py

@@ -1,33 +1,49 @@
 from setuptools import setup
 
-package_name = 'webots_ros2_tests'
+package_name = "webots_ros2_tests"
 
 data_files = [
-    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-    ('share/' + package_name, ['package.xml']),
-    ('share/' + package_name + '/worlds', ['worlds/driver_test.wbt', 'worlds/.driver_test.wbproj']),
-    ('share/' + package_name + '/resource', ['resource/driver_test.urdf'])
+    ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+    ("share/" + package_name, ["package.xml"]),
+    (
+        "share/" + package_name + "/worlds",
+        [
+            "worlds/driver_test.wbt",
+            "worlds/.driver_test.wbproj",
+            "worlds/connector_and_vacuum_gripper_test.wbt",
+            "worlds/.connector_and_vacuum_gripper_test.wbproj",
+        ],
+    ),
+    (
+        "share/" + package_name + "/resource",
+        [
+            "resource/driver_test.urdf",
+            "resource/connector_and_vacuum_gripper_test.urdf",
+            "resource/object_position.urdf.xacro",
+            "resource/connector_and_vacuum_gripper_test_ros2_control.yml",
+        ],
+    ),
 ]
 
 setup(
     name=package_name,
-    version='2025.0.0',
+    version="2025.0.0",
     packages=[package_name],
     data_files=data_files,
-    install_requires=['setuptools', 'launch'],
+    install_requires=["setuptools", "launch"],
     zip_safe=True,
-    author='Cyberbotics',
-    author_email='[email protected]',
-    maintainer='Cyberbotics',
-    maintainer_email='[email protected]',
-    keywords=['ROS', 'Webots', 'Robot', 'Simulation', 'Examples'],
+    author="Cyberbotics",
+    author_email="[email protected]",
+    maintainer="Cyberbotics",
+    maintainer_email="[email protected]",
+    keywords=["ROS", "Webots", "Robot", "Simulation", "Examples"],
     classifiers=[
-        'Intended Audience :: Developers',
-        'License :: OSI Approved :: Apache Software License',
-        'Programming Language :: Python',
-        'Topic :: Software Development',
+        "Intended Audience :: Developers",
+        "License :: OSI Approved :: Apache Software License",
+        "Programming Language :: Python",
+        "Topic :: Software Development",
     ],
-    description='System tests for `webots_ros2` packages',
-    license='Apache License, Version 2.0',
-    tests_require=['pytest']
+    description="System tests for `webots_ros2` packages",
+    license="Apache License, Version 2.0",
+    tests_require=["pytest"],
 )