Release/Flexiv ROS 2 Humble v1.5.1 (#41)

README.md

@@ -195,7 +195,7 @@ The robot driver (`rizon.launch.py`) publishes the following feedback states to
 
 ### GPIO
 
-All digital inputs on the robot control box can be accessed via the ROS topic `/gpio_controller/gpio_inputs`, which publishes the current state of all the 16 digital input ports *(True: port high, false: port low)*.
+All digital inputs on the robot control box can be accessed via the ROS topic `/gpio_controller/gpio_inputs`, which publishes the current state of all the 18 *(16 on control box + 2 inside the wrist connector)* digital input ports *(True: port high, false: port low)*.
 
 The digital output ports on the control box can be set by publishing to the topic `/gpio_controller/gpio_outputs`. For example:
 

flexiv_controllers/gpio_controller/include/gpio_controller/gpio_controller.hpp

@@ -17,6 +17,10 @@
 #include "flexiv_msgs/msg/gpio_states.hpp"
 
 namespace gpio_controller {
+
+/** Number of digital IO ports (16 on control box + 2 inside the wrist connector) */
+constexpr size_t kIOPorts = 18;
+
 using CmdType = flexiv_msgs::msg::GPIOStates;
 
 class GPIOController : public controller_interface::ControllerInterface
@@ -43,7 +47,7 @@ class GPIOController : public controller_interface::ControllerInterface
     void initMsgs();
 
     // internal commands
-    std::array<double, 16> digital_outputs_cmd_;
+    std::array<double, kIOPorts> digital_outputs_cmd_;
 
     // publisher
     std::shared_ptr<rclcpp::Publisher<CmdType>> gpio_inputs_publisher_;

flexiv_controllers/gpio_controller/src/gpio_controller.cpp

@@ -33,7 +33,7 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
     controller_interface::InterfaceConfiguration config;
     config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 
-    for (size_t i = 0; i < 16; ++i) {
+    for (size_t i = 0; i < kIOPorts; ++i) {
         config.names.emplace_back("gpio/digital_output_" + std::to_string(i));
     }
 
@@ -45,7 +45,7 @@ controller_interface::InterfaceConfiguration GPIOController::state_interface_con
     controller_interface::InterfaceConfiguration config;
     config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 
-    for (size_t i = 0; i < 16; ++i) {
+    for (size_t i = 0; i < kIOPorts; ++i) {
         config.names.emplace_back("gpio/digital_input_" + std::to_string(i));
     }
 
@@ -56,7 +56,7 @@ controller_interface::return_type GPIOController::update(
     const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
 {
     // get inputs
-    for (size_t i = 0; i < 16; ++i) {
+    for (size_t i = 0; i < kIOPorts; ++i) {
         gpio_inputs_msg_.states[i].pin = i;
         gpio_inputs_msg_.states[i].state = static_cast<bool>(state_interfaces_[i].get_value());
     }
@@ -82,7 +82,7 @@ controller_interface::CallbackReturn GPIOController::on_configure(
         gpio_outputs_command_ = get_node()->create_subscription<CmdType>(
             "~/gpio_outputs", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) {
                 for (size_t i = 0; i < msg->states.size(); ++i) {
-                    if (msg->states[i].pin >= 16) {
+                    if (msg->states[i].pin >= kIOPorts) {
                         RCLCPP_WARN(get_node()->get_logger(),
                             "Received command for pin %d, but only pins 0-15 are supported.",
                             msg->states[i].pin);

flexiv_description/urdf/rizon10.ros2_control.xacro

@@ -143,6 +143,7 @@
         <state_interface name="effort"/>
       </joint>
 
+      <!-- 18 digital IO ports (16 on control box + 2 inside the wrist connector) -->
       <gpio name="${prefix}gpio">
         <command_interface name="digital_output_0"/>
         <command_interface name="digital_output_1"/>
@@ -160,6 +161,8 @@
         <command_interface name="digital_output_13"/>
         <command_interface name="digital_output_14"/>
         <command_interface name="digital_output_15"/>
+        <command_interface name="digital_output_16"/>
+        <command_interface name="digital_output_17"/>
 
         <state_interface name="digital_input_0"/>
         <state_interface name="digital_input_1"/>
@@ -177,6 +180,8 @@
         <state_interface name="digital_input_13"/>
         <state_interface name="digital_input_14"/>
         <state_interface name="digital_input_15"/>
+        <state_interface name="digital_input_16"/>
+        <state_interface name="digital_input_17"/>
       </gpio>
 
     </ros2_control>

flexiv_description/urdf/rizon4.ros2_control.xacro

@@ -143,6 +143,7 @@
         <state_interface name="effort"/>
       </joint>
 
+      <!-- 18 digital IO ports (16 on control box + 2 inside the wrist connector) -->
       <gpio name="${prefix}gpio">
         <command_interface name="digital_output_0"/>
         <command_interface name="digital_output_1"/>
@@ -160,6 +161,8 @@
         <command_interface name="digital_output_13"/>
         <command_interface name="digital_output_14"/>
         <command_interface name="digital_output_15"/>
+        <command_interface name="digital_output_16"/>
+        <command_interface name="digital_output_17"/>
 
         <state_interface name="digital_input_0"/>
         <state_interface name="digital_input_1"/>
@@ -177,6 +180,8 @@
         <state_interface name="digital_input_13"/>
         <state_interface name="digital_input_14"/>
         <state_interface name="digital_input_15"/>
+        <state_interface name="digital_input_16"/>
+        <state_interface name="digital_input_17"/>
       </gpio>
 
     </ros2_control>

flexiv_hardware/CMakeLists.txt

@@ -23,12 +23,6 @@ if(spdlog_FOUND)
   message(STATUS "Found spdlog: ${spdlog_DIR}")
 endif()
 
-# Fast-CDR
-find_package(fastcdr 1.0.24 REQUIRED)
-if(fastcdr_FOUND)
-  message(STATUS "Found fastcdr: ${fastcdr_DIR}")
-endif()
-
 # Fast-DDS (Fast-RTPS)
 find_package(fastrtps 2.6.2 REQUIRED)
 if(fastrtps_FOUND)
@@ -77,7 +71,6 @@ target_link_libraries(${PROJECT_NAME}
   ${RDK_STATIC_LIBRARY}
   spdlog::spdlog
   fastrtps
-  fastcdr
 )
 
 # Link ROS packages