feat(robot_driver): add tool GPIO get/set to RobotDriver

examples/sas_robot_driver_example_main.cpp

@@ -63,6 +63,15 @@ int main(int,char**)
 
     assert((target_joint_positions == robot_driver_example.get_joint_positions()));
 
+    // Tool GPIO: default-initialised to false, and the base implementation stores what is set.
+    if(robot_driver_example.get_tool_gpio() != std::array<bool, 2>{false, false})
+        throw std::runtime_error("sas_robot_driver_example_main::Tool GPIO should default to {false, false}.");
+    robot_driver_example.set_tool_gpio({true, false});
+    if(robot_driver_example.get_tool_gpio() != std::array<bool, 2>{true, false})
+        throw std::runtime_error("sas_robot_driver_example_main::Tool GPIO round-trip failed.");
+    std::cout << "Tool GPIO after change: " << robot_driver_example.get_tool_gpio()[0] << " "
+              << robot_driver_example.get_tool_gpio()[1] << std::endl;
+
     robot_driver_example.deinitialize();
     robot_driver_example.disconnect();
 

include/marinholab/sas/core/sas_robot_driver.hpp

@@ -27,6 +27,8 @@
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      Added the Watchdog functionaly initially proposed in
 #      https://github.com/SmartArmStack/sas_core/pull/1
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 */
 
 /**
@@ -37,6 +39,7 @@
  * implement to interact with hardware. The header also contains watchdog
  * management used by driver implementations.
  */
+#include <array>
 #include <atomic>
 #include <mutex>
 #include <thread>
@@ -63,6 +66,7 @@ class RobotDriver
     std::tuple<Eigen::VectorXd, Eigen::VectorXd> joint_limits_;
     Eigen::VectorXd joint_velocities_;
     Eigen::VectorXd joint_torques_;
+    std::array<bool, 2> tool_gpio_{};
 
     std::unique_ptr<marinholab::sas::core::Clock> clock_;
     std::unique_ptr<std::thread> watchdog_thread_;
@@ -158,6 +162,20 @@ class RobotDriver
      */
     virtual void set_joint_limits(const std::tuple<Eigen::VectorXd, Eigen::VectorXd>& joint_limits);
 
+    /**
+     * @brief Get the last commanded tool (wrist) digital output values
+     * @note The default implementation returns the value stored by set_tool_gpio(). Drivers may override it.
+     * @return Array of boolean values, one per digital pin
+     */
+    virtual std::array<bool, 2> get_tool_gpio();
+
+    /**
+     * @brief Set the tool (wrist) digital output values
+     * @note The default implementation only stores the value. Drivers override it to command the hardware.
+     * @param tool_gpio Array of boolean values, one per digital pin
+     */
+    virtual void set_tool_gpio(const std::array<bool, 2>& tool_gpio);
+
     /**
      * @brief Start the watchdog thread with the given period
      * @param period Watchdog period as nanoseconds

src/sas_robot_driver.cpp

@@ -25,6 +25,8 @@
 #   1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk)
 #      Added the Watchdog functionality initially proposed in
 #      https://github.com/SmartArmStack/sas_core/pull/1
+#   2. Erwin Lopez (erwin.lopez@manchester.ac.uk)
+#      Added functionality to control tool gpio
 #*/
 
 /**
@@ -90,6 +92,16 @@ void RobotDriver::set_joint_limits(const std::tuple<Eigen::VectorXd, Eigen::Vect
     joint_limits_ = joint_limits;
 }
 
+std::array<bool, 2> RobotDriver::get_tool_gpio()
+{
+    return tool_gpio_;
+}
+
+void RobotDriver::set_tool_gpio(const std::array<bool, 2>& tool_gpio)
+{
+    tool_gpio_ = tool_gpio;
+}
+
 
 /**
  * @brief RobotDriver::_watchdog_thread_function throws an exception if the elapsed time since the last trigger