Fix ee pose broadcaster (#14)

* add ee_br to launch

* expand hw value getter (RQ: does not solve the actual error)

* fix ee_broadcaster:
  - explicit joint names
  - fix HW map access
  - add orientation (not optimized...)
  - test for NaN
  - test for insufficient joints

* clean-up + CI compliance

RQ: successfully tested on hardware (Omega 3 and Omega 6).

fd_bringup/launch/fd.launch.py

@@ -90,7 +90,7 @@ def generate_launch_description():
 
     # Load controllers
     load_controllers = []
-    for controller in ['fd_controller', 'joint_state_broadcaster']:
+    for controller in ['fd_controller', 'joint_state_broadcaster', 'fd_ee_broadcaster']:
         load_controllers += [
             Node(
                 package='controller_manager',

fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp

@@ -69,6 +69,7 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface
   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::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
 
   Eigen::Matrix4d transform_, pose_;

fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp

@@ -56,6 +56,7 @@ EePoseBroadcaster::on_init()
   try {
     // 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<double>>("transform_translation", std::vector<double>());
     auto_declare<std::vector<double>>("transform_rotation", std::vector<double>());
   } catch (const std::exception & e) {
@@ -76,13 +77,30 @@ EePoseBroadcaster::command_interface_configuration() const
 controller_interface::InterfaceConfiguration EePoseBroadcaster::state_interface_configuration()
 const
 {
-  return controller_interface::InterfaceConfiguration{
-    controller_interface::interface_configuration_type::ALL};
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  if (joints_.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "No joint name provided!");
+
+  } else {
+    for (const auto & joint : joints_) {
+      state_interfaces_config.names.push_back(joint + "/" + HW_IF_POSITION);
+    }
+  }
+
+  return state_interfaces_config;
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  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!");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
   auto transform_trans_param = get_node()->get_parameter("transform_translation").as_double_array();
   auto transform_rot_param = get_node()->get_parameter("transform_rotation").as_double_array();
   Eigen::Quaternion<double> q;
@@ -139,12 +157,20 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 EePoseBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  if (state_interfaces_.size() != (joints_.size())) {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Not all requested interfaces exists.");
+  }
+
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 EePoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  joints_.clear();
+
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -166,18 +192,50 @@ controller_interface::return_type EePoseBroadcaster::update(
   const rclcpp::Duration & /*period*/)
 {
   for (const auto & state_interface : state_interfaces_) {
-    name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] =
+    name_if_value_mapping_[
+      state_interface.get_prefix_name()][state_interface.get_interface_name()] =
       state_interface.get_value();
     RCLCPP_DEBUG(
-      get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(),
+      get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_prefix_name().c_str(),
       state_interface.get_interface_name().c_str(), state_interface.get_value());
   }
 
   if (realtime_ee_pose_publisher_ && realtime_ee_pose_publisher_->trylock()) {
     pose_ = Eigen::Matrix4d::Identity();
-    pose_(0, 3) = get_value(name_if_value_mapping_, "fd_x", HW_IF_POSITION);
-    pose_(1, 3) = get_value(name_if_value_mapping_, "fd_y", HW_IF_POSITION);
-    pose_(2, 3) = get_value(name_if_value_mapping_, "fd_z", HW_IF_POSITION);
+
+    if (joints_.size() >= 3) {
+      double p_x = get_value(name_if_value_mapping_, joints_[0], HW_IF_POSITION);
+      double p_y = get_value(name_if_value_mapping_, joints_[1], HW_IF_POSITION);
+      double p_z = get_value(name_if_value_mapping_, joints_[2], HW_IF_POSITION);
+
+      if (std::isnan(p_x) || std::isnan(p_y) || std::isnan(p_z)) {
+        RCLCPP_DEBUG(
+          get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!");
+        return controller_interface::return_type::ERROR;
+      }
+      pose_(0, 3) = p_x;
+      pose_(1, 3) = p_y;
+      pose_(2, 3) = p_z;
+    }
+
+    if (joints_.size() >= 6) {
+      double roll = get_value(name_if_value_mapping_, joints_[3], HW_IF_POSITION);
+      double pitch = get_value(name_if_value_mapping_, joints_[4], HW_IF_POSITION);
+      double yaw = get_value(name_if_value_mapping_, joints_[5], HW_IF_POSITION);
+
+      if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) {
+        RCLCPP_DEBUG(
+          get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!");
+        return controller_interface::return_type::ERROR;
+      }
+
+      Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
+      Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
+      Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
+
+      Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle;
+      pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
+    }
 
     pose_ = transform_ * pose_;
     Eigen::Quaternion<double> q(pose_.block<3, 3>(0, 0));

fd_description/config/fd_controllers.yaml

@@ -5,6 +5,9 @@ fd/controller_manager:
     fd_controller:
       type: effort_controllers/JointGroupEffortController # ForwardCommandController
 
+    fd_ee_broadcaster:
+      type: ee_pose_broadcaster/EePoseBroadcaster
+
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
@@ -20,8 +23,28 @@ fd/fd_controller:
       - fd_x
       - fd_y
       - fd_z
-    ## Note: uncomment the following if using an omega 6/7 device
-    #  - fd_roll
-    #  - fd_pitch
-    #  - fd_yaw
-    #  - fd_clutch
+    ## Note: uncomment the following if using an omega 7 device (e.g., 7 controlled DoF)
+      # - fd_roll
+      # - fd_pitch
+      # - fd_yaw
+    ## Note: uncomment the following if a clutch is present (e.g., omega 6/7 device)
+      # - fd_clutch
+
+fd/fd_ee_broadcaster:
+  ros__parameters:
+    transform_translation:
+      - 0.0
+      - 0.0
+      - 0.0
+    transform_rotation:
+      - 0.0
+      - 0.0
+      - 0.0
+    joints:
+      - fd_x
+      - fd_y
+      - fd_z
+    ## Note: uncomment the following if orientation is enabled (e.g., omega 6/7 device)
+      # - fd_roll
+      # - fd_pitch
+      # - fd_yaw