diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt index 693d9eda9..94ded60e1 100644 --- a/webots_ros2_driver/CMakeLists.txt +++ b/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 ) diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Connector.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Connector.hpp new file mode 100644 index 000000000..75c6db070 --- /dev/null +++ b/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 +#include + +#include +#include +#include + +namespace webots_ros2_driver { + class Ros2Connector : public Ros2SensorPlugin { + public: + void step() override; + void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; + + private: + void publishPresence(); + + // ROS2 subscriptions + rclcpp::Subscription::SharedPtr mLockSubscription; + void LockCallback(const std_msgs::msg::Bool::SharedPtr message); + + // ROS2 services + rclcpp::Service::SharedPtr mLockService; + void isLockedCallback(const std::shared_ptr request, + std::shared_ptr response); + + // ROS2 topics + rclcpp::Publisher::SharedPtr mPresencePublisher; + webots_ros2_msgs::msg::IntStamped mPresenceMessage; + bool mIsPresenceEnabled; + + // Device + WbDeviceTag mConnector; + // Runtime vars + std::string mDeviceName; + }; + +} // namespace webots_ros2_driver + +#endif \ No newline at end of file diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp index 62e5f1c71..566fd1d58 100644 --- a/webots_ros2_driver/src/WebotsNode.cpp +++ b/webots_ros2_driver/src/WebotsNode.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -261,6 +262,9 @@ namespace webots_ros2_driver { case WB_NODE_VACUUM_GRIPPER: plugin = std::make_shared(); break; + case WB_NODE_CONNECTOR: + plugin = std::make_shared(); + break; } if (plugin) { plugin->init(this, parameters); diff --git a/webots_ros2_driver/src/plugins/static/Ros2Connector.cpp b/webots_ros2_driver/src/plugins/static/Ros2Connector.cpp new file mode 100644 index 000000000..c84233e68 --- /dev/null +++ b/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 +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +namespace webots_ros2_driver { + void Ros2Connector::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { + 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( + mTopicName + "/lock", rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2Connector::LockCallback, this, _1)); + + mLockService = mNode->create_service( + mTopicName + "/is_locked", std::bind(&Ros2Connector::isLockedCallback, this, _1, _2)); + + mIsPresenceEnabled = false; + mPresencePublisher = + mNode->create_publisher(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 request, + std::shared_ptr 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 diff --git a/webots_ros2_msgs/CMakeLists.txt b/webots_ros2_msgs/CMakeLists.txt index cb9b18b12..b08de5cc2 100644 --- a/webots_ros2_msgs/CMakeLists.txt +++ b/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" diff --git a/webots_ros2_msgs/msg/IntStamped.msg b/webots_ros2_msgs/msg/IntStamped.msg new file mode 100644 index 000000000..bc4cf3260 --- /dev/null +++ b/webots_ros2_msgs/msg/IntStamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +int8 data \ No newline at end of file