diff --git a/AGENTS.md b/AGENTS.md index b306a7e..ef9c494 100644 --- a/AGENTS.md +++ b/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 + `/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. diff --git a/README.md b/README.md index b33aaa7..af5e246 100644 --- a/README.md +++ b/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)` (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 | +|--------------------------------|-------------------------------|-----------------|---------------------------------------------------| +| `/set/tool_gpio` | `std_msgs/msg/ByteMultiArray` | client → server | Desired value per tool pin (`data[i]` is 0 or 1) | + ### Importing in Python ```python diff --git a/include/sas_robot_driver/sas_robot_driver_client.hpp b/include/sas_robot_driver/sas_robot_driver_client.hpp index 511ac0e..5861946 100644 --- a/include/sas_robot_driver/sas_robot_driver_client.hpp +++ b/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 @@ -36,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -92,6 +94,7 @@ class RobotDriverClient: private sas::Object rclcpp::Publisher ::SharedPtr publisher_clear_positions_signal_; rclcpp::Publisher ::SharedPtr publisher_watchdog_trigger_; rclcpp::Publisher ::SharedPtr publisher_shutdown_signal_; + rclcpp::Publisher ::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& tool_gpio); + /** * @brief Get the last received joint positions. * diff --git a/include/sas_robot_driver/sas_robot_driver_ros.hpp b/include/sas_robot_driver/sas_robot_driver_ros.hpp index 67619fd..af9bb74 100644 --- a/include/sas_robot_driver/sas_robot_driver_ros.hpp +++ b/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 q_max; + + /// Enables forwarding of tool GPIO commands from the server to the robot driver. + bool robot_tool_gpio_enable{false}; }; /** diff --git a/include/sas_robot_driver/sas_robot_driver_server.hpp b/include/sas_robot_driver/sas_robot_driver_server.hpp index d4940f6..f34d0b8 100644 --- a/include/sas_robot_driver/sas_robot_driver_server.hpp +++ b/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 @@ -35,6 +36,7 @@ #include #include #include +#include #include #include @@ -67,7 +69,8 @@ class RobotDriverServer: private sas::Object rclcpp::Publisher::SharedPtr publisher_joint_limits_max_; rclcpp::Publisher::SharedPtr publisher_home_state_; - + rclcpp::Subscription::SharedPtr subscriber_tool_gpio_; + std::array tool_gpio_{}; rclcpp::Subscription::SharedPtr subscriber_shutdown_signal_; bool shutdown_signal_; rclcpp::Subscription::SharedPtr subscriber_target_joint_positions_; @@ -88,6 +91,7 @@ class RobotDriverServer: private sas::Object std::chrono::time_point time_point_from_the_client_; std::chrono::time_point 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 digital value of gpio pins. + */ + std::array get_tool_gpio() const; + /** * @brief Check whether the server supports and is enabled for a functionality. * diff --git a/src/sas_robot_driver_client.cpp b/src/sas_robot_driver_client.cpp index 65eed88..97c2e29 100755 --- a/src/sas_robot_driver_client.cpp +++ b/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 @@ -124,6 +125,7 @@ RobotDriverClient::RobotDriverClient(const std::shared_ptr &node, } // All client types can shut down the server. publisher_shutdown_signal_ = node->create_publisher(topic_prefix + "/set/shutdown", 1); + publisher_tool_gpio_ = node->create_publisher(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 &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(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()) diff --git a/src/sas_robot_driver_py.cpp b/src/sas_robot_driver_py.cpp index 1633ae1..56d26ad 100644 --- a/src/sas_robot_driver_py.cpp +++ b/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); } diff --git a/src/sas_robot_driver_ros.cpp b/src/sas_robot_driver_ros.cpp index 5bbaf92..4f0dd5d 100644 --- a/src/sas_robot_driver_ros.cpp +++ b/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 @@ -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!"); diff --git a/src/sas_robot_driver_server.cpp b/src/sas_robot_driver_server.cpp index e79a42d..12d25da 100755 --- a/src/sas_robot_driver_server.cpp +++ b/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 @@ -37,6 +38,27 @@ using std::placeholders::_1; namespace sas { +std::array 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(msg.data[i]); +} void RobotDriverServer::_callback_shutdown_signal_(const sas_msgs::msg::Bool &msg) { @@ -162,6 +184,9 @@ RobotDriverServer::RobotDriverServer(const std::shared_ptr &node, subscriber_shutdown_signal_ = node->create_subscription( topic_prefix + "/set/shutdown", 1, std::bind(&RobotDriverServer::_callback_shutdown_signal_, this, _1) ); + subscriber_tool_gpio_ = node->create_subscription( + topic_prefix + "/set/tool_gpio", 1, std::bind(&RobotDriverServer::_callback_tool_gpio, this, _1) + ); } Eigen::VectorXd RobotDriverServer::get_target_joint_positions() const