Add switch to publish the controllers' state feedback

Implements #111 for ROS2.

cartesian_controller_base/CMakeLists.txt

@@ -32,6 +32,7 @@ find_package(kdl_parser REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(urdf REQUIRED)
+find_package(realtime_tools REQUIRED)
 
 
 # Convenience variable for dependencies
@@ -42,6 +43,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
         pluginlib
         urdf
         Eigen3
+        realtime_tools
 )
 
 ament_export_dependencies(

cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h

@@ -47,13 +47,16 @@
 #include <cartesian_controller_base/Utility.h>
 #include <controller_interface/controller_interface.hpp>
 #include <functional>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <hardware_interface/loaned_command_interface.hpp>
 #include <hardware_interface/loaned_state_interface.hpp>
 #include <kdl/treefksolverpos_recursive.hpp>
 #include <memory>
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <realtime_tools/realtime_publisher.h>
 #include <string>
 #include <trajectory_msgs/msg/joint_trajectory_point.hpp>
 #include <vector>
@@ -185,6 +188,23 @@ class CartesianControllerBase : public controller_interface::ControllerInterface
       m_joint_state_pos_handles;
 
   private:
+
+    /**
+     * @brief Publish the controller's end-effector pose and twist
+     *
+     * The data are w.r.t. the specified robot base link.
+     * If this function is called after `computeJointControlCmds()` has
+     * been called, then the controller's internal state represents the state
+     * right after the error computation, and corresponds to the new target
+     * state that will be send to the actuators in this control cycle.
+     */
+    void publishStateFeedback();
+
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::msg::PoseStamped>
+      m_feedback_pose_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::msg::TwistStamped>
+      m_feedback_twist_publisher;
+
     std::vector<std::string> m_cmd_interface_types;
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> m_joint_cmd_pos_handles;
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> m_joint_cmd_vel_handles;

cartesian_controller_base/package.xml

@@ -16,6 +16,7 @@
   <depend>kdl_parser</depend>
   <depend>trajectory_msgs</depend>
   <depend>pluginlib</depend>
+  <depend>realtime_tools</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

cartesian_controller_base/src/cartesian_controller_base.cpp

@@ -39,6 +39,8 @@
 
 #include "controller_interface/controller_interface.hpp"
 #include "controller_interface/helpers.hpp"
+#include "geometry_msgs/msg/detail/pose_stamped__struct.hpp"
+#include "geometry_msgs/msg/detail/twist_stamped__struct.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include <cartesian_controller_base/cartesian_controller_base.h>
@@ -96,6 +98,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
     auto_declare<std::vector<std::string>>("command_interfaces", std::vector<std::string>());
     auto_declare<double>("solver.error_scale", 1.0);
     auto_declare<int>("solver.iterations", 1);
+    auto_declare<bool>("solver.publish_state_feedback", false);
     m_initialized = true;
   }
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -121,6 +124,7 @@ controller_interface::return_type CartesianControllerBase::init(const std::strin
     auto_declare<std::vector<std::string>>("command_interfaces", std::vector<std::string>());
     auto_declare<double>("solver.error_scale", 1.0);
     auto_declare<int>("solver.iterations", 1);
+    auto_declare<bool>("solver.publish_state_feedback", false);
 
     m_initialized = true;
   }
@@ -255,6 +259,17 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
     }
   }
 
+  // Controller-internal state publishing
+  m_feedback_pose_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped> >(
+      get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
+        std::string(get_node()->get_name()) + "/current_pose", 3));
+
+  m_feedback_twist_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped> >(
+      get_node()->create_publisher<geometry_msgs::msg::TwistStamped>(
+        std::string(get_node()->get_name()) + "/current_twist", 3));
+
   m_configured = true;
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -335,6 +350,11 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
 
 void CartesianControllerBase::writeJointControlCmds()
 {
+  if (get_node()->get_parameter("solver.publish_state_feedback").as_bool())
+  {
+    publishStateFeedback();
+  }
+
   // Write all available types.
   for (const auto& type : m_cmd_interface_types)
   {
@@ -456,4 +476,43 @@ ctrl::Vector6D CartesianControllerBase::displayInTipLink(const ctrl::Vector6D& v
   return out;
 }
 
+void CartesianControllerBase::publishStateFeedback()
+{
+  // End-effector pose
+  auto pose = m_ik_solver->getEndEffectorPose();
+  if (m_feedback_pose_publisher->trylock()){
+    m_feedback_pose_publisher->msg_.header.stamp = get_node()->now();
+    m_feedback_pose_publisher->msg_.header.frame_id = m_robot_base_link;
+    m_feedback_pose_publisher->msg_.pose.position.x = pose.p.x();
+    m_feedback_pose_publisher->msg_.pose.position.y = pose.p.y();
+    m_feedback_pose_publisher->msg_.pose.position.z = pose.p.z();
+
+    pose.M.GetQuaternion(
+        m_feedback_pose_publisher->msg_.pose.orientation.x,
+        m_feedback_pose_publisher->msg_.pose.orientation.y,
+        m_feedback_pose_publisher->msg_.pose.orientation.z,
+        m_feedback_pose_publisher->msg_.pose.orientation.w
+        );
+
+    m_feedback_pose_publisher->unlockAndPublish();
+  }
+
+  // End-effector twist
+  auto twist = m_ik_solver->getEndEffectorVel();
+  if (m_feedback_twist_publisher->trylock()){
+    m_feedback_twist_publisher->msg_.header.stamp = get_node()->now();
+    m_feedback_twist_publisher->msg_.header.frame_id = m_robot_base_link;
+    m_feedback_twist_publisher->msg_.twist.linear.x = twist[0];
+    m_feedback_twist_publisher->msg_.twist.linear.y = twist[1];
+    m_feedback_twist_publisher->msg_.twist.linear.z = twist[2];
+    m_feedback_twist_publisher->msg_.twist.angular.x = twist[3];
+    m_feedback_twist_publisher->msg_.twist.angular.y = twist[4];
+    m_feedback_twist_publisher->msg_.twist.angular.z = twist[5];
+
+    m_feedback_twist_publisher->unlockAndPublish();
+  }
+
+}
+
+
 } // namespace

cartesian_controller_simulation/config/controller_manager.yaml

@@ -90,6 +90,7 @@ cartesian_compliance_controller:
     solver:
         error_scale: 0.5
         iterations: 1
+        publish_state_feedback: True
 
     # For all controllers, these gains are w.r.t. the robot_base_link coordinates.
     pd_gains:
@@ -122,6 +123,7 @@ cartesian_force_controller:
 
     solver:
         error_scale: 0.5
+        publish_state_feedback: True
 
     pd_gains:
         trans_x: {p: 0.05}
@@ -153,6 +155,7 @@ cartesian_motion_controller:
     solver:
         error_scale: 1.0
         iterations: 10
+        publish_state_feedback: True
 
     pd_gains:
         trans_x: {p: 1.0}

resources/doc/Solver_details.md

@@ -39,7 +39,7 @@ Unfortunately, there won't exist ideal parameters for every use case and robot.
 So, for your specific application, you will be tweaking the PD gains at some point.
 
 ### Solver parameters
-The common solver has two parameters:
+The common solver has several parameters:
 * **iterations**: The number of internally simulated cycles per control cycle.
   Increasing this number will give the solver more iterations to approach the given target.
   A value of `10` is a good default for the `CartesianMotionController` and the `CartesianComplianceController`.
@@ -51,11 +51,39 @@ The common solver has two parameters:
   Use this parameter to find the right range
   for your PD gains. It's handy to use the slider in dynamic reconfigure for this.
 
+* **publish_state_feedback**: A boolean flag whether to publish the
+  controller-internal state. This is helpful for comparing the controllers'
+  feedback against the given target reference during parameter tweaking. If
+  `true`, each controller will publish its end-effector pose and twist on the local topics `current_pose` and
+  `current_twist`, respectively. You can easily find them in a sourced terminal with
+  ```bash
+  ros2 topic list | grep current
+  ```
+
+All solver parameters can be set online via `dynamic_reconfigure` in the controllers'
+`solver` namespace, or at startup via the controller's `.yaml` configuration
+file, e.g. with
+```yaml
+my_cartesian_controller:
+  ros__parameters:
+
+    # link and joints specification here
+    # ...
+
+    solver:
+        error_scale: 0.5
+        iterations: 5
+        publish_state_feedback: True
+
+    # Further specification
+    # ...
+```
+
 ## Performance
 As a default, please build the cartesian_controllers in release mode:
 
 ```bash
-catkin_make -DCMAKE_BUILD_TYPE=Release
+colcon build --packages-skip cartesian_controller_simulation cartesian_controller_tests --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 The forward dynamics implementation heavily relies on
 orocos_kinematics_dynamics (KDL), which use Eigen for linear algebra.