do not discard 'set_value' return values on newer 'hardware_interface'

boschresearch · Dec 5, 2024 · 18b416e · 18b416e
1 parent e705c3d
commit 18b416e
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 0 deletions.
diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
@@ -54,6 +54,10 @@ generate_parameter_library(franka_example_controllers_parameters src/model_examp
 
 target_link_libraries(${PROJECT_NAME} franka_example_controllers_parameters Franka::Franka)
 
+if(${hardware_interface_VERSION} VERSION_GREATER_EQUAL "4.20")
+    target_compile_definitions(${PROJECT_NAME} PRIVATE HW_HAS_SET_NODISCARD)
+endif()
+
 pluginlib_export_plugin_description_file(
         controller_interface franka_example_controllers.xml)
 

diff --git a/franka_example_controllers/src/gravity_compensation_example_controller.cpp b/franka_example_controllers/src/gravity_compensation_example_controller.cpp
@@ -39,7 +39,12 @@ controller_interface::return_type GravityCompensationExampleController::update(
     const rclcpp::Time& /*time*/,
     const rclcpp::Duration& /*period*/) {
   for (auto& command_interface : command_interfaces_) {
+#ifdef HW_HAS_SET_NODISCARD
+    if (!command_interface.set_value(0))
+      return controller_interface::return_type::ERROR;
+#else
     command_interface.set_value(0);
+#endif
   }
   return controller_interface::return_type::OK;
 }

diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
@@ -60,7 +60,12 @@ controller_interface::return_type JointImpedanceExampleController::update(
   Vector7d tau_d_calculated =
       k_gains_.cwiseProduct(q_goal - q_) + d_gains_.cwiseProduct(-dq_filtered_);
   for (int i = 0; i < num_joints; ++i) {
+#ifdef HW_HAS_SET_NODISCARD
+    if (!command_interfaces_[i].set_value(tau_d_calculated(i)))
+      return controller_interface::return_type::ERROR;
+#else
     command_interfaces_[i].set_value(tau_d_calculated(i));
+#endif
   }
   return controller_interface::return_type::OK;
 }

diff --git a/franka_example_controllers/src/move_to_start_example_controller.cpp b/franka_example_controllers/src/move_to_start_example_controller.cpp
@@ -59,11 +59,21 @@ controller_interface::return_type MoveToStartExampleController::update(
     Vector7d tau_d_calculated =
         k_gains_.cwiseProduct(q_desired - q_) + d_gains_.cwiseProduct(-dq_filtered_);
     for (int i = 0; i < 7; ++i) {
+#ifdef HW_HAS_SET_NODISCARD
+      if (!command_interfaces_[i].set_value(tau_d_calculated(i)))
+        return controller_interface::return_type::ERROR;
+#else
       command_interfaces_[i].set_value(tau_d_calculated(i));
+#endif
     }
   } else {
     for (auto& command_interface : command_interfaces_) {
+#ifdef HW_HAS_SET_NODISCARD
+      if (!command_interface.set_value(0))
+        return controller_interface::return_type::ERROR;
+#else
       command_interface.set_value(0);
+#endif
     }
     this->get_node()->set_parameter({"process_finished", true});
   }