FRI 2.x compilation #156

lbr-stack · Feb 23, 2024 · 35be1ff · 35be1ff
1 parent 4b3a9a0
commit 35be1ff
Show file tree

Hide file tree

Showing 13 changed files with 104 additions and 14 deletions.
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp
@@ -9,6 +9,7 @@
 #include "rclcpp/logging.hpp"
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 #include "lbr_fri_ros2/command_interface.hpp"
 #include "lbr_fri_ros2/enum_maps.hpp"

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
@@ -11,6 +11,7 @@
 
 #include "friLBRClient.h"
 #include "friLBRState.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp
@@ -10,9 +10,11 @@
 #include "rclcpp/logging.hpp"
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_ros2/command_guard.hpp"
+#include "lbr_fri_ros2/enum_maps.hpp"
 #include "lbr_fri_ros2/filters.hpp"
 
 namespace lbr_fri_ros2 {

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp
@@ -4,6 +4,7 @@
 #include <string>
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 namespace lbr_fri_ros2 {
 struct EnumMaps {
@@ -43,8 +44,16 @@ struct EnumMaps {
     switch (client_command_mode) {
     case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE:
       return "NO_COMMAND_MODE";
+#if FRICLIENT_VERSION_MAJOR == 1
     case KUKA::FRI::EClientCommandMode::POSITION:
       return "POSITION";
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+      return "JOINT_POSITION";
+    case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE:
+      return "CARTESIAN_POSE";
+#endif
     case KUKA::FRI::EClientCommandMode::TORQUE:
       return "TORQUE";
     case KUKA::FRI::EClientCommandMode::WRENCH:

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp
@@ -7,6 +7,7 @@
 #include "rclcpp/logging.hpp"
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/filters.hpp"

diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp
@@ -144,7 +144,12 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host,
 
     // subscriptions
     switch (async_client_ptr_->get_state_interface().get_state().client_command_mode) {
+#if FRICLIENT_VERSION_MAJOR == 1
     case KUKA::FRI::EClientCommandMode::POSITION:
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+#endif
       position_command_sub_ =
           app_node_ptr_->create_subscription<lbr_fri_msgs::msg::LBRPositionCommand>(
               "command/joint_position", 1,
@@ -169,9 +174,15 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host,
 
 void AppComponent::on_position_command_(
     const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr lbr_position_command) {
-  if (!on_command_checks_(KUKA::FRI::EClientCommandMode::POSITION)) {
-    return;
-  }
+#if FRICLIENT_VERSION_MAJOR == 1
+  if (!on_command_checks_(KUKA::FRI::EClientCommandMode::POSITION))
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    if (!on_command_checks_(KUKA::FRI::EClientCommandMode::JOINT_POSITION))
+#endif
+    {
+      return;
+    }
 
   if (async_client_ptr_->get_state_interface().get_state().session_state ==
       KUKA::FRI::ESessionState::COMMANDING_ACTIVE) {

diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp
@@ -8,6 +8,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "urdf/model.h"
 
+#include "friVersion.h"
+
 #include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 #include "lbr_fri_msgs/msg/lbr_torque_command.hpp"

diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp
@@ -49,20 +49,39 @@ void AsyncClient::command() {
   }
 
   switch (robotState().getClientCommandMode()) {
+#if FRICLIENT_VERSION_MAJOR == 1
   case KUKA::FRI::EClientCommandMode::POSITION:
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+#endif
+  {
     command_interface_.get_joint_position_command(robotCommand(), robotState());
     return;
-  case KUKA::FRI::EClientCommandMode::TORQUE:
+  }
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: {
+    std::string err =
+        EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) +
+        " command mode not supported yet.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+#endif
+  case KUKA::FRI::EClientCommandMode::TORQUE: {
     command_interface_.get_torque_command(robotCommand(), robotState());
     return;
-  case KUKA::FRI::EClientCommandMode::WRENCH:
+  }
+  case KUKA::FRI::EClientCommandMode::WRENCH: {
     command_interface_.get_wrench_command(robotCommand(), robotState());
     return;
-  default:
+  }
+  default: {
     std::string err =
         "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + ".";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
+  }
 }
 } // end of namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp
@@ -9,14 +9,23 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command,
   switch (lbr_state.getClientCommandMode()) {
   case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE:
     return false;
+#if FRICLIENT_VERSION_MAJOR == 1
   case KUKA::FRI::EClientCommandMode::POSITION:
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+#endif
     if (!command_in_position_limits_(lbr_command, lbr_state)) {
       return false;
     }
     if (!command_in_velocity_limits_(lbr_command, lbr_state)) {
       return false;
     }
     return true;
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE:
+    return false;
+#endif
   case KUKA::FRI::EClientCommandMode::WRENCH:
     if (!command_in_position_limits_(lbr_command, lbr_state)) {
       return false;

diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp
@@ -11,11 +11,25 @@ CommandInterface::CommandInterface(const PIDParameters &pid_parameters,
 
 void CommandInterface::get_joint_position_command(fri_command_t_ref command,
                                                   const_fri_state_t_ref state) {
+#if FRICLIENT_VERSION_MAJOR == 1
   if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) {
-    std::string err = "Set joint position only allowed in position command mode.";
+    std::string err = "Set joint position only allowed in " +
+                      EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) +
+                      " command mode.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+  if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::JOINT_POSITION) {
+    std::string err =
+        "Set joint position only allowed in " +
+        EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) +
+        " command mode.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+#endif
   if (!command_guard_) {
     std::string err = "Uninitialized command guard.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());

diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp
@@ -6,8 +6,10 @@ StateInterface::StateInterface(const StateInterfaceParameters &state_interface_p
 
 void StateInterface::set_state(const_fri_state_t_ref state) {
   state_.client_command_mode = state.getClientCommandMode();
+#if FRICLIENT_VERSION_MAJOR == 1
   std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
+#endif
   std::memcpy(state_.commanded_torque.data(), state.getCommandedTorque(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
   state_.connection_quality = state.getConnectionQuality();
@@ -41,8 +43,10 @@ void StateInterface::set_state(const_fri_state_t_ref state) {
 void StateInterface::set_state_open_loop(const_fri_state_t_ref state,
                                          const_idl_joint_pos_t_ref joint_position) {
   state_.client_command_mode = state.getClientCommandMode();
+#if FRICLIENT_VERSION_MAJOR == 1
   std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
+#endif
   std::memcpy(state_.commanded_torque.data(), state.getCommandedTorque(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
   state_.connection_quality = state.getConnectionQuality();

diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
@@ -16,6 +16,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 
 #include "friLBRState.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"

diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp
@@ -272,14 +272,21 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
   if (hw_session_state_ != KUKA::FRI::COMMANDING_ACTIVE) {
     return hardware_interface::return_type::OK;
   }
-  if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) {
-    if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(),
-                    [](const double &v) { return std::isnan(v); })) {
+#if FRICLIENT_VERSION_MAJOR == 1
+  if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION)
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::JOINT_POSITION)
+#endif
+    {
+      if (std::any_of(hw_lbr_command_.joint_position.cbegin(),
+                      hw_lbr_command_.joint_position.cend(),
+                      [](const double &v) { return std::isnan(v); })) {
+        return hardware_interface::return_type::OK;
+      }
+      async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
       return hardware_interface::return_type::OK;
     }
-    async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
-    return hardware_interface::return_type::OK;
-  }
   if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) {
     if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(),
                     [](const double &v) { return std::isnan(v); }) ||
@@ -291,8 +298,17 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
     return hardware_interface::return_type::OK;
   }
   if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) {
-    throw std::runtime_error("Wrench command mode currently not implemented.");
+    throw std::runtime_error(
+        lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) +
+        " command mode currently not implemented.");
+  }
+#if FRICLIENT_VERSION_MAJOR == 2
+  if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) {
+    throw std::runtime_error(
+        lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) +
+        " command mode currently not implemented.");
   }
+#endif
   return hardware_interface::return_type::ERROR;
 }