Add button as gpio

fd_controllers/ee_pose_broadcaster/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(realtime_tools REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3)
 
@@ -33,6 +34,7 @@ ament_target_dependencies(ee_pose_broadcaster
   rcutils
   realtime_tools
   geometry_msgs
+  std_msgs
   eigen3_cmake_module
   Eigen3
 )
@@ -62,6 +64,7 @@ ament_export_dependencies(
   geometry_msgs
   eigen3_cmake_module
   Eigen3
+  example_interfaces
 )
 ament_export_include_directories(
   include

fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp

@@ -29,6 +29,7 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "realtime_tools/realtime_publisher.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "std_msgs/msg/bool.hpp"
 
 namespace ee_pose_broadcaster
 {
@@ -65,14 +66,20 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface
     const rclcpp_lifecycle::State & previous_state) override;
 
 protected:
-  //  For the PoseStamped message,
-  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>> ee_pose_publisher_;
-  std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>
-  realtime_ee_pose_publisher_;
   std::vector<std::string> joints_;
+  std::vector<std::string> buttons_;
   std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
 
   Eigen::Matrix4d transform_, pose_;
+
+  //  Publishers
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>> ee_pose_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>
+  realtime_ee_pose_publisher_;
+
+  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> fd_button_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>
+  realtime_fd_button_publisher_;
 };
 
 }  // namespace ee_pose_broadcaster

fd_controllers/ee_pose_broadcaster/package.xml

@@ -20,8 +20,9 @@
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
   <depend>rclcpp_lifecycle</depend>
-  <depend>geometry_msgs</depend>
   <depend>realtime_tools</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>

fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp

@@ -57,6 +57,7 @@ EePoseBroadcaster::on_init()
     // definition of the parameters that need to be queried from the
     // controller configuration file with default values
     auto_declare<std::vector<std::string>>("joints", std::vector<std::string>());
+    auto_declare<std::vector<std::string>>("buttons", std::vector<std::string>());
     auto_declare<std::vector<double>>("transform_translation", std::vector<double>());
     auto_declare<std::vector<double>>("transform_rotation", std::vector<double>());
   } catch (const std::exception & e) {
@@ -89,12 +90,22 @@ const
     }
   }
 
+  if (buttons_.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "No button name provided!");
+
+  } else {
+    for (const auto & button : buttons_) {
+      state_interfaces_config.names.push_back(button + "/" + HW_IF_POSITION);
+    }
+  }
+
   return state_interfaces_config;
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  buttons_ = get_node()->get_parameter("buttons").as_string_array();
   joints_ = get_node()->get_parameter("joints").as_string_array();
   if (joints_.empty()) {
     RCLCPP_ERROR(get_node()->get_logger(), "Please provide list of joints in config!");
@@ -137,7 +148,6 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state
 
   std::cout << transform_ << std::endl;
 
-
   try {
     ee_pose_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
       "ee_pose",
@@ -146,6 +156,13 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state
     realtime_ee_pose_publisher_ =
       std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>(
       ee_pose_publisher_);
+
+    fd_button_publisher_ = get_node()->create_publisher<std_msgs::msg::Bool>(
+      "button_state", rclcpp::SystemDefaultsQoS());
+
+    realtime_fd_button_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(
+      fd_button_publisher_);
   } catch (const std::exception & e) {
     // get_node() may throw, logging raw here
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
@@ -256,6 +273,15 @@ controller_interface::return_type EePoseBroadcaster::update(
     realtime_ee_pose_publisher_->unlockAndPublish();
   }
 
+  if (!buttons_.empty()) {
+    if (realtime_fd_button_publisher_ && realtime_fd_button_publisher_->trylock()) {
+      auto & ee_button_msg = realtime_fd_button_publisher_->msg_;
+      double button_status = get_value(name_if_value_mapping_, buttons_[0], HW_IF_POSITION);
+      ee_button_msg.data = button_status > 0.5;
+      realtime_fd_button_publisher_->unlockAndPublish();
+    }
+  }
+
   return controller_interface::return_type::OK;
 }
 

fd_description/config/fd_controllers.yaml

@@ -48,3 +48,4 @@ fd/fd_ee_broadcaster:
       # - fd_roll
       # - fd_pitch
       # - fd_yaw
+    buttons: [button]  # Note: only one button supported for now

fd_description/ros2_control/fd.r2c_hardware.xacro

@@ -12,6 +12,11 @@
                 </xacro:unless>
             </hardware>
 
+            <!-- Configure GPIOs -->
+            <gpio name="button">
+                <state_interface name="position"/>
+            </gpio>
+
             <!-- Configure translations -->
             <xacro:macro name="configure_translation_joint" params="joint_name">
                 <joint name="${joint_name}">

fd_hardware/include/fd_hardware/fd_effort_hi.hpp

@@ -67,6 +67,7 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface
   std::vector<double> hw_states_position_;
   std::vector<double> hw_states_velocity_;
   std::vector<double> hw_states_effort_;
+  std::vector<double> hw_button_state_;
 
   /**
   Initiate the USB communication with the device.

fd_hardware/src/fd_effort_hi.cpp

@@ -48,6 +48,7 @@ CallbackReturn FDEffortHardwareInterface::on_init(
   hw_states_velocity_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_states_effort_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_commands_effort_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+  hw_button_state_.resize(info_.gpios.size(), std::numeric_limits<double>::quiet_NaN());
 
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints) {
@@ -98,6 +99,38 @@ CallbackReturn FDEffortHardwareInterface::on_init(
       return CallbackReturn::ERROR;
     }
   }
+  for (const hardware_interface::ComponentInfo & button : info_.gpios) {
+    if (button.state_interfaces.size() != 1) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("FDEffortHardwareInterface"),
+        "Button '%s' has %lu state interface. 1 expected.", button.name.c_str(),
+        button.state_interfaces.size());
+      return CallbackReturn::ERROR;
+    }
+    if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("FDEffortHardwareInterface"),
+        "Button '%s' have %s state interface. '%s' expected.", button.name.c_str(),
+        button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+      return CallbackReturn::ERROR;
+    }
+  }
+  for (const hardware_interface::ComponentInfo & button : info_.gpios) {
+    if (button.state_interfaces.size() != 1) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("FDEffortHardwareInterface"),
+        "Button '%s' has %lu state interface. 1 expected.", button.name.c_str(),
+        button.state_interfaces.size());
+      return CallbackReturn::ERROR;
+    }
+    if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("FDEffortHardwareInterface"),
+        "Button '%s' have %s state interface. '%s' expected.", button.name.c_str(),
+        button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+      return CallbackReturn::ERROR;
+    }
+  }
 
   return CallbackReturn::SUCCESS;
 }
@@ -121,6 +154,11 @@ FDEffortHardwareInterface::export_state_interfaces()
       hardware_interface::StateInterface(
         info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
   }
+  for (uint i = 0; i < info_.gpios.size(); i++) {
+    state_interfaces.emplace_back(
+      hardware_interface::StateInterface(
+        info_.gpios[i].name, hardware_interface::HW_IF_POSITION, &hw_button_state_[i]));
+  }
 
   return state_interfaces;
 }
@@ -221,6 +259,16 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
     hw_states_effort_[6] = gripper_force;
   }
 
+  // Get button status, TODO multiple buttons from index
+  int button_status = dhdGetButton(0, interface_ID_);
+  if (button_status == 1) {
+    hw_button_state_[0] = 1.0;
+  } else if (button_status == 0) {
+    hw_button_state_[0] = 0.0;
+  } else {
+    flag += -1;
+  }
+
   if (flag >= 0) {
     return hardware_interface::return_type::OK;
   } else {