bundling types in shared header

lbr-stack · Oct 22, 2024 · 079232b · 079232b
1 parent bb31bcc
commit 079232b
Show file tree

Hide file tree

Showing 20 changed files with 102 additions and 80 deletions.
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
@@ -17,13 +17,10 @@
 #include "lbr_fri_idl/msg/lbr_command.hpp"
 #include "lbr_fri_idl/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/formatting.hpp"
+#include "lbr_fri_ros2/types.hpp"
 
 namespace lbr_fri_ros2 {
 struct CommandGuardParameters {
-  // ROS IDL types
-  using jnt_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
-  using jnt_name_array_t = std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
-
   jnt_name_array_t joint_names;                           /**< Joint names.*/
   jnt_array_t min_positions{0., 0., 0., 0., 0., 0., 0.};  /**< Minimum joint positions [rad].*/
   jnt_array_t max_positions{0., 0., 0., 0., 0., 0., 0.};  /**< Maximum joint positions [rad].*/
@@ -35,14 +32,6 @@ class CommandGuard {
 protected:
   static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard";
 
-  // ROS IDL types
-  using idl_command_t = lbr_fri_idl::msg::LBRCommand;
-  using const_idl_command_t_ref = const idl_command_t &;
-  using idl_state_t = lbr_fri_idl::msg::LBRState;
-  using const_idl_state_t_ref = const idl_state_t &;
-  using jnt_array_t = idl_command_t::_joint_position_type;
-  using const_jnt_array_t_ref = const jnt_array_t &;
-
 public:
   CommandGuard() = default;
   CommandGuard(const CommandGuardParameters &command_guard_parameters);

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp
@@ -12,6 +12,7 @@
 #include "friLBRClient.h"
 
 #include "lbr_fri_idl/msg/lbr_state.hpp"
+#include "lbr_fri_ros2/types.hpp"
 
 namespace lbr_fri_ros2 {
 class ExponentialFilter {
@@ -96,12 +97,10 @@ class ExponentialFilter {
 };
 
 class JointExponentialFilterArray {
-  using value_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
-
 public:
   JointExponentialFilterArray() = default;
 
-  void compute(const double *const current, value_array_t &previous);
+  void compute(const double *const current, jnt_array_t_ref previous);
   void initialize(const double &cutoff_frequency, const double &sample_time);
   inline const bool &is_initialized() const { return initialized_; };
 
@@ -122,17 +121,16 @@ struct PIDParameters {
 class JointPIDArray {
 protected:
   static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray";
-  using value_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
   using pid_array_t = std::array<control_toolbox::Pid, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
 
 public:
   JointPIDArray() = delete;
   JointPIDArray(const PIDParameters &pid_parameters);
 
-  void compute(const value_array_t &command_target, const value_array_t &state,
-               const std::chrono::nanoseconds &dt, value_array_t &command);
-  void compute(const value_array_t &command_target, const double *state,
-               const std::chrono::nanoseconds &dt, value_array_t &command);
+  void compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state,
+               const std::chrono::nanoseconds &dt, jnt_array_t_ref command);
+  void compute(const_jnt_array_t_ref command_target, const double *state,
+               const std::chrono::nanoseconds &dt, jnt_array_t_ref command);
   void log_info() const;
 
 protected:

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp
@@ -16,27 +16,19 @@
 #include "lbr_fri_idl/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/kinematics.hpp"
 #include "lbr_fri_ros2/pinv.hpp"
+#include "lbr_fri_ros2/types.hpp"
 
 namespace lbr_fri_ros2 {
 class FTEstimator {
 protected:
   static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator";
-  using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type;
-  using const_jnt_pos_array_t_ref = const jnt_pos_array_t &;
-  using ext_tau_array_t = lbr_fri_idl::msg::LBRState::_external_torque_type;
-  using const_ext_tau_array_t_ref = const ext_tau_array_t &;
 
 public:
-  using cart_array_t = std::array<double, Kinematics::CARTESIAN_DOF>;
-  using cart_array_t_ref = cart_array_t &;
-  using const_cart_array_t_ref = const cart_array_t &;
-
   FTEstimator(const std::string &robot_description, const std::string &chain_root = "lbr_link_0",
               const std::string &chain_tip = "lbr_link_ee",
               const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5});
-  void compute(const_jnt_pos_array_t_ref measured_joint_position,
-               const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext,
-               const double &damping = 0.2);
+  void compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque,
+               cart_array_t_ref f_ext, const double &damping = 0.2);
   void reset();
 
 protected:
@@ -47,10 +39,9 @@ class FTEstimator {
   std::unique_ptr<Kinematics> kinematics_ptr_;
 
   // force estimation
-  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, Kinematics::CARTESIAN_DOF>
-      jacobian_inv_;
+  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, CARTESIAN_DOF> jacobian_inv_;
   Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1> tau_ext_;
-  Eigen::Matrix<double, Kinematics::CARTESIAN_DOF, 1> f_ext_;
+  Eigen::Matrix<double, CARTESIAN_DOF, 1> f_ext_;
 };
 } // namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp
@@ -15,26 +15,23 @@
 #include "friLBRState.h"
 
 #include "lbr_fri_idl/msg/lbr_state.hpp"
+#include "lbr_fri_ros2/types.hpp"
 
 namespace lbr_fri_ros2 {
 class Kinematics {
 protected:
   static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics";
 
 public:
-  static constexpr uint8_t CARTESIAN_DOF = 6;
-  using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type;
-  using const_jnt_pos_array_t_ref = const jnt_pos_array_t &;
-
   Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0",
              const std::string &chain_tip = "lbr_link_ee");
 
   // internally computes the jacobian and return a reference to it
-  const KDL::Jacobian &compute_jacobian(const_jnt_pos_array_t_ref q);
+  const KDL::Jacobian &compute_jacobian(const_jnt_array_t_ref q);
   const KDL::Jacobian &compute_jacobian(const std::vector<double> &q);
 
   // forward kinematics
-  const KDL::Frame &compute_fk(const_jnt_pos_array_t_ref q);
+  const KDL::Frame &compute_fk(const_jnt_array_t_ref q);
   const KDL::Frame &compute_fk(const std::vector<double> &q);
 
 protected:

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp
@@ -0,0 +1,40 @@
+#ifndef LBR_FRI_ROS2__TYPES_HPP_
+#define LBR_FRI_ROS2__TYPES_HPP_
+
+#include <array>
+#include <cstdint>
+#include <string>
+
+#include "friLBRState.h"
+
+#include "lbr_fri_idl/msg/lbr_command.hpp"
+#include "lbr_fri_idl/msg/lbr_state.hpp"
+
+namespace lbr_fri_ros2 {
+// joint positions, velocities, accelerations, torques etc.
+using jnt_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+using jnt_array_t_ref = jnt_array_t &;
+using const_jnt_array_t_ref = const jnt_array_t &;
+
+// joint names
+using jnt_name_array_t = std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+using jnt_name_array_t_ref = jnt_name_array_t &;
+using const_jnt_name_array_t_ref = const jnt_name_array_t &;
+
+// cartesian dof
+constexpr std::uint8_t CARTESIAN_DOF = 6;
+
+// cartesian positions, velocities, accelerations, torques etc.
+using cart_array_t = std::array<double, CARTESIAN_DOF>;
+using cart_array_t_ref = cart_array_t &;
+using const_cart_array_t_ref = const cart_array_t &;
+
+// idl types
+using idl_command_t = lbr_fri_idl::msg::LBRCommand;
+using idl_command_t_ref = idl_command_t &;
+using const_idl_command_t_ref = const idl_command_t &;
+using idl_state_t = lbr_fri_idl::msg::LBRState;
+using idl_state_t_ref = idl_state_t &;
+using const_idl_state_t_ref = const idl_state_t &;
+} // namespace lbr_fri_ros2
+#endif // LBR_FRI_ROS2__TYPES_HPP_
diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp
@@ -107,7 +107,8 @@ void App::run_async(int rt_prio) {
     should_stop_ = false;
     bool success = true;
     while (rclcpp::ok() && success && !should_stop_) {
-      success = app_ptr_->step(); // stuck if never connected
+      success = app_ptr_->step(); // stuck if never connected, we thus detach the thread as join may
+                                  // never return
       running_ = true;
       if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) {
         RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting");

diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp
@@ -2,7 +2,7 @@
 
 namespace lbr_fri_ros2 {
 CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters)
-    : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false){};
+    : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false) {};
 
 bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command,
                                     const_idl_state_t_ref lbr_state) {

diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp
@@ -29,7 +29,7 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency,
 
 bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; }
 
-void JointExponentialFilterArray::compute(const double *const current, value_array_t &previous) {
+void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) {
   std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS,
                 [&, i = 0](const auto &current_i) mutable {
                   previous[i] = exponential_filter_.compute(current_i, previous[i]);
@@ -54,16 +54,16 @@ JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters)
   });
 }
 
-void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state,
-                            const std::chrono::nanoseconds &dt, value_array_t &command) {
+void JointPIDArray::compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state,
+                            const std::chrono::nanoseconds &dt, jnt_array_t_ref command) {
   std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable {
     command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count());
     ++i;
   });
 }
 
-void JointPIDArray::compute(const value_array_t &command_target, const double *state,
-                            const std::chrono::nanoseconds &dt, value_array_t &command) {
+void JointPIDArray::compute(const_jnt_array_t_ref command_target, const double *state,
+                            const std::chrono::nanoseconds &dt, jnt_array_t_ref command) {
   std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable {
     command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count());
     ++i;

diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp
@@ -8,8 +8,8 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string
   reset();
 }
 
-void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
-                          const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext,
+void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position,
+                          const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext,
                           const double &damping) {
   tau_ext_ = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
       external_torque.data());
@@ -22,7 +22,7 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
   f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3);
   f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3);
 
-  Eigen::Map<Eigen::Matrix<double, Kinematics::CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
+  Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
 
   // threshold force-torque
   std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(),

diff --git a/lbr_fri_ros2/src/kinematics.cpp b/lbr_fri_ros2/src/kinematics.cpp
@@ -25,7 +25,7 @@ Kinematics::Kinematics(const std::string &robot_description, const std::string &
   q_.data.setZero();
 }
 
-const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_pos_array_t_ref q) {
+const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) {
   q_.data =
       Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
   jacobian_solver_->JntToJac(q_, jacobian_);
@@ -44,7 +44,7 @@ const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector<double> &q)
   return jacobian_;
 }
 
-const KDL::Frame &Kinematics::compute_fk(const_jnt_pos_array_t_ref q) {
+const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) {
   q_.data =
       Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
   fk_solver_->JntToCart(q_, chain_tip_frame_);

diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp
@@ -14,12 +14,13 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "friLBRState.h"
+#include "lbr_fri_ros2/types.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {
 class Admittance {
-
-}
+  // implement an addmittance...
+};
 
 class AdmittanceController : public controller_interface::ControllerInterface {
   static constexpr uint8_t CARTESIAN_DOF = 6;
@@ -52,7 +53,7 @@ class AdmittanceController : public controller_interface::ControllerInterface {
   void clear_state_interfaces_();
   void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
+  lbr_fri_ros2::jnt_name_array_t joint_names_;
 
   std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_position_command_interfaces_;

diff --git a/...s2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp b/...s2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp
@@ -15,6 +15,7 @@
 #include "friLBRState.h"
 
 #include "lbr_fri_idl/msg/lbr_joint_position_command.hpp"
+#include "lbr_fri_ros2/types.hpp"
 
 namespace lbr_ros2_control {
 class LBRJointPositionCommandController : public controller_interface::ControllerInterface {
@@ -42,7 +43,7 @@ class LBRJointPositionCommandController : public controller_interface::Controlle
 protected:
   void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
+  lbr_fri_ros2::jnt_name_array_t joint_names_;
 
   realtime_tools::RealtimeBuffer<lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr>
       rt_lbr_joint_position_command_ptr_;

diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp
@@ -19,6 +19,7 @@
 #include "friLBRState.h"
 
 #include "lbr_fri_idl/msg/lbr_state.hpp"
+#include "lbr_fri_ros2/types.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {
@@ -49,8 +50,7 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface {
   void init_state_msg_();
   void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
-      "lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"};
+  lbr_fri_ros2::jnt_name_array_t joint_names_;
   std::unordered_map<std::string, std::unordered_map<std::string, double>> state_interface_map_;
 
   rclcpp::Publisher<lbr_fri_idl::msg::LBRState>::SharedPtr state_publisher_ptr_;

diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp
@@ -17,6 +17,7 @@
 #include "friLBRState.h"
 
 #include "lbr_fri_idl/msg/lbr_torque_command.hpp"
+#include "lbr_fri_ros2/types.hpp"
 
 namespace lbr_ros2_control {
 class LBRTorqueCommandController : public controller_interface::ControllerInterface {
@@ -46,7 +47,7 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf
   void clear_command_interfaces_();
   void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
+  lbr_fri_ros2::jnt_name_array_t joint_names_;
 
   std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_position_command_interfaces_, torque_command_interfaces_;

diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp
@@ -17,6 +17,7 @@
 #include "friLBRState.h"
 
 #include "lbr_fri_idl/msg/lbr_wrench_command.hpp"
+#include "lbr_fri_ros2/types.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {
@@ -49,7 +50,7 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf
   void clear_command_interfaces_();
   void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
+  lbr_fri_ros2::jnt_name_array_t joint_names_;
 
   std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_position_command_interfaces_, wrench_command_interfaces_;