Add tool GPIO support to RobotDriverServer/Client and RobotDriverROS

AGENTS.md

@@ -77,6 +77,10 @@ interface example. Make sure changes keep that flow green.
   states and joint limits from its server (`is_enabled()`); servers that
   wait on a client should send `send_joint_states()` / `send_joint_limits()`
   while spinning.
+- Tool GPIO: `RobotDriverClient::send_tool_gpio()` publishes on
+  `<topic_prefix>/set/tool_gpio` (`std_msgs/ByteMultiArray`, 2 elements);
+  `RobotDriverROS` forwards it to `RobotDriver::set_tool_gpio()` only when
+  `RobotDriverROSConfiguration::robot_tool_gpio_enable` is `true`.
 - Mode blacklisting: `RobotDriverClient` accepts `blacklisted_modes`
   (`MODE_BLACKLIST_FLAG`, e.g. `JOINT_CONTROL`) to disable functionality
   from the client side — the watchdog commander node uses this.

README.md

@@ -32,6 +32,17 @@ forms the namespace for all topics.  When `topic_prefix` is `"GET_FROM_NODE"`
 | `sas::RobotDriverServer` | `sas_robot_driver/sas_robot_driver_server.hpp` |
 | `sas::RobotDriverClient` | `sas_robot_driver/sas_robot_driver_client.hpp` |
 
+### Tool GPIO
+
+The client can command the digital outputs on the robot's tool connector with
+`send_tool_gpio(std::array<bool, 2>)` (Python: `send_tool_gpio([bool, bool])`).
+The value is forwarded to `RobotDriver::set_tool_gpio()` only when
+`RobotDriverROSConfiguration::robot_tool_gpio_enable` is `true` (default `false`).
+
+| Topic                          | Type                          | Direction       | Description                                       |
+|--------------------------------|-------------------------------|-----------------|---------------------------------------------------|
+| `<topic_prefix>/set/tool_gpio` | `std_msgs/msg/ByteMultiArray` | client → server | Desired value per tool pin (`data[i]` is 0 or 1)  |
+
 ### Importing in Python
 
 ```python

include/sas_robot_driver/sas_robot_driver_client.hpp

@@ -26,7 +26,8 @@
 #
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      Added the Watchdog functionality.
-#
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 #include <atomic>
@@ -36,6 +37,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <std_msgs/msg/float64_multi_array.hpp>
 #include <std_msgs/msg/int32_multi_array.hpp>
+#include <std_msgs/msg/byte_multi_array.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <sas_msgs/msg/watchdog_trigger.hpp>
 #include <sas_core/sas_robot_driver.hpp>
@@ -92,6 +94,7 @@ class RobotDriverClient: private sas::Object
     rclcpp::Publisher<std_msgs::msg::Int32MultiArray>  ::SharedPtr publisher_clear_positions_signal_;
     rclcpp::Publisher<sas_msgs::msg::WatchdogTrigger>  ::SharedPtr publisher_watchdog_trigger_;
     rclcpp::Publisher<sas_msgs::msg::Bool>             ::SharedPtr publisher_shutdown_signal_;
+    rclcpp::Publisher<std_msgs::msg::ByteMultiArray>   ::SharedPtr publisher_tool_gpio_;
 
     void _callback_joint_states(const sensor_msgs::msg::JointState& msg);
     void _callback_joint_limits_min(const std_msgs::msg::Float64MultiArray& msg);
@@ -163,6 +166,13 @@ class RobotDriverClient: private sas::Object
      */
     void send_shutdown_signal();
 
+    /**
+     * @brief Send digital values for tool gpio to the robot.
+     *
+     * @param tool_gpio Array of bool representing a digital value per pin.
+     */
+    void send_tool_gpio(const std::array<bool, 2>& tool_gpio);
+
     /**
      * @brief Get the last received joint positions.
      *

include/sas_robot_driver/sas_robot_driver_ros.hpp

@@ -27,7 +27,8 @@
 #      - Added the Watchdog functionality.
 #      - Renamed robot_driver_provider_ to robot_driver_server_
 #      - Added a new std::optional parameter in RobotDriverROSConfiguration to define the watchdog period
-#
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 #pragma once
@@ -69,6 +70,9 @@ struct RobotDriverROSConfiguration
 
     /// Joint position maximum limits (q_max.size() == number of joints).
     std::vector<double> q_max;
+
+    /// Enables forwarding of tool GPIO commands from the server to the robot driver.
+    bool robot_tool_gpio_enable{false};
 };
 
 /**

include/sas_robot_driver/sas_robot_driver_server.hpp

@@ -26,7 +26,8 @@
 #
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      Added the Watchdog functionality.
-#
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 #include <tuple>
@@ -35,6 +36,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <std_msgs/msg/float64_multi_array.hpp>
 #include <std_msgs/msg/int32_multi_array.hpp>
+#include <std_msgs/msg/byte_multi_array.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 
 #include <sas_core/sas_robot_driver.hpp>
@@ -67,7 +69,8 @@ class RobotDriverServer: private sas::Object
     rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_joint_limits_max_;
     rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr publisher_home_state_;
 
-
+    rclcpp::Subscription<std_msgs::msg::ByteMultiArray>::SharedPtr subscriber_tool_gpio_;
+    std::array<bool, 2> tool_gpio_{};
     rclcpp::Subscription<sas_msgs::msg::Bool>::SharedPtr subscriber_shutdown_signal_;
     bool shutdown_signal_;
     rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_positions_;
@@ -88,6 +91,7 @@ class RobotDriverServer: private sas::Object
     std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> time_point_from_the_client_;
     std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> time_point_from_the_server_;
 
+    void _callback_tool_gpio(const std_msgs::msg::ByteMultiArray& msg);
     void _callback_shutdown_signal_(const sas_msgs::msg::Bool& msg);
     void _callback_target_joint_positions(const std_msgs::msg::Float64MultiArray &msg);
     void _callback_target_joint_velocities(const std_msgs::msg::Float64MultiArray &msg);
@@ -159,6 +163,13 @@ class RobotDriverServer: private sas::Object
      */
     RobotDriver::Functionality get_currently_active_functionality() const;
 
+    /**
+     * @brief Get the tool gpio digital values.
+     *
+     * @return std::array<bool, 2> digital value of gpio pins.
+     */
+    std::array<bool, 2> get_tool_gpio() const;
+
     /**
      * @brief Check whether the server supports and is enabled for a functionality.
      *

src/sas_robot_driver_client.cpp

@@ -25,7 +25,8 @@
 #
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      Added the Watchdog functionality.
-#
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 #include <sas_robot_driver/sas_robot_driver_client.hpp>
@@ -124,6 +125,7 @@ RobotDriverClient::RobotDriverClient(const std::shared_ptr<rclcpp::Node> &node,
     }
     // All client types can shut down the server.
     publisher_shutdown_signal_ = node->create_publisher<sas_msgs::msg::Bool>(topic_prefix + "/set/shutdown", 1);
+    publisher_tool_gpio_ = node->create_publisher<std_msgs::msg::ByteMultiArray>(topic_prefix + "/set/tool_gpio", 1);
 }
 
 void RobotDriverClient::send_target_joint_positions(const Eigen::VectorXd &target_joint_positions)
@@ -226,6 +228,27 @@ void RobotDriverClient::send_shutdown_signal()
     publisher_shutdown_signal_->publish(ros_msg);
 }
 
+void RobotDriverClient::send_tool_gpio(const std::array<bool, 2> &tool_gpio)
+{
+    std_msgs::msg::ByteMultiArray ros_msg;
+    ros_msg.data.resize(tool_gpio.size());
+
+    for(std::size_t i = 0; i < tool_gpio.size(); ++i)
+    {
+        ros_msg.data[i] = static_cast<uint8_t>(tool_gpio[i]);
+    }
+
+    std_msgs::msg::MultiArrayDimension dim;
+    dim.label = "tool_gpio";
+    dim.size = tool_gpio.size();
+    dim.stride = tool_gpio.size();
+
+    ros_msg.layout.dim.push_back(dim);
+    ros_msg.layout.data_offset = 0;
+
+    publisher_tool_gpio_->publish(ros_msg);
+}
+
 Eigen::VectorXd RobotDriverClient::get_joint_positions() const
 {
     if(is_enabled())

src/sas_robot_driver_py.cpp

@@ -65,6 +65,7 @@ PYBIND11_MODULE(_sas_robot_driver, m) {
             .def("send_target_joint_forces",&RDC::send_target_joint_forces)
             .def("send_homing_signal",&RDC::send_homing_signal)
             .def("send_clear_positions_signal",&RDC::send_clear_positions_signal)
+            .def("send_tool_gpio",&RDC::send_tool_gpio)
             .def("get_joint_positions",&RDC::get_joint_positions)
             .def("get_joint_velocities",&RDC::get_joint_velocities)
             .def("get_joint_forces",&RDC::get_joint_forces)
@@ -81,6 +82,7 @@ PYBIND11_MODULE(_sas_robot_driver, m) {
             .def("get_homing_signal",&RDS::get_homing_signal)
             .def("get_clear_positions_signal",&RDS::get_clear_positions_signal)
             .def("get_currently_active_functionality",&RDS::get_currently_active_functionality)
+            .def("get_tool_gpio",&RDS::get_tool_gpio)
             .def("is_enabled",&RDS::is_enabled,"Returns true if the RobotDriverProvider is enabled.",py::arg("supported_functionality")=sas::RobotDriver::Functionality::PositionControl)
             .def("send_joint_states",&RDS::send_joint_states)
             .def("send_joint_limits",&RDS::send_joint_limits)
@@ -99,5 +101,6 @@ PYBIND11_MODULE(_sas_robot_driver, m) {
          .def_readwrite("thread_sampling_time_sec", &RDRC::thread_sampling_time_sec)
          .def_readwrite("watchdog_period_in_seconds", &RDRC::watchdog_period_in_seconds)
          .def_readwrite("q_min", &RDRC::q_min)
-         .def_readwrite("q_max", &RDRC::q_max);
+         .def_readwrite("q_max", &RDRC::q_max)
+         .def_readwrite("robot_tool_gpio_enable", &RDRC::robot_tool_gpio_enable);
 }

src/sas_robot_driver_ros.cpp

@@ -26,7 +26,8 @@
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      - Added the Watchdog functionality.
 #      - Renamed robot_driver_provider_ to robot_driver_server_
-#
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 #include <sas_common/sas_common.hpp>
@@ -181,7 +182,10 @@ int RobotDriverROS::control_loop()
 
             }
 
-
+            if(configuration_.robot_tool_gpio_enable)
+            {
+                robot_driver_->set_tool_gpio(robot_driver_server_.get_tool_gpio());
+            }
             // Execute the control loop callback if one has been set
             if (robot_driver_->control_loop_callback_is_set()) {
                 RCLCPP_INFO_STREAM_ONCE(node_->get_logger(), "::Control loop callback is set and will be executed!");

src/sas_robot_driver_server.cpp

@@ -25,7 +25,8 @@
 #
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      Added the Watchdog functionality.
-#
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 #include <rclcpp/rclcpp.hpp>
@@ -37,6 +38,27 @@ using std::placeholders::_1;
 namespace sas
 {
 
+std::array<bool, 2> RobotDriverServer::get_tool_gpio() const
+{
+    return tool_gpio_;
+}
+
+void RobotDriverServer::_callback_tool_gpio(const std_msgs::msg::ByteMultiArray &msg)
+{
+    const std::string this_topic(node_prefix_ + "/set/tool_gpio");
+    if(node_->count_publishers(this_topic)>1)
+        throw std::runtime_error(this_topic + " must be exclusively published and there is more than one publisher connected.");
+
+    if(msg.data.size() != tool_gpio_.size())
+    {
+        RCLCPP_WARN_STREAM(node_->get_logger(), "::Ignoring " << this_topic << " message with " << msg.data.size()
+                                                << " elements. Expected " << tool_gpio_.size() << ".");
+        return;
+    }
+
+    for(std::size_t i = 0; i < tool_gpio_.size(); ++i)
+        tool_gpio_[i] = static_cast<bool>(msg.data[i]);
+}
 
 void RobotDriverServer::_callback_shutdown_signal_(const sas_msgs::msg::Bool &msg)
 {
@@ -162,6 +184,9 @@ RobotDriverServer::RobotDriverServer(const std::shared_ptr<rclcpp::Node> &node,
     subscriber_shutdown_signal_ = node->create_subscription<sas_msgs::msg::Bool>(
          topic_prefix + "/set/shutdown", 1, std::bind(&RobotDriverServer::_callback_shutdown_signal_, this, _1)
         );
+    subscriber_tool_gpio_ = node->create_subscription<std_msgs::msg::ByteMultiArray>(
+         topic_prefix + "/set/tool_gpio", 1, std::bind(&RobotDriverServer::_callback_tool_gpio, this, _1)
+        );
 }
 
 Eigen::VectorXd RobotDriverServer::get_target_joint_positions() const