Publish wrench state feedback

cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h

@@ -103,6 +103,23 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
     std::string           m_new_ft_sensor_ref;
     void setFtSensorReferenceFrame(const std::string& new_ref);
 
+    /**
+     * @brief Publish the controller's state wrench
+     *
+     * 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.
+     */
+    /**
+     * @brief Publish the controller's wrenches (sensor, gravity, target and net force)
+     *
+     * The data are w.r.t. the specified robot base link.
+     */
+    void publishStateWrenchFeedback(realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>& rt_publisher,
+                                    ctrl::Vector6D& wrench);
+
   private:
     ctrl::Vector6D        compensateGravity();
 
@@ -136,6 +153,15 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
 
     std::shared_ptr<dynamic_reconfigure::Server<Config> > m_dyn_conf_server;
     dynamic_reconfigure::Server<Config>::CallbackType m_callback_type;
+
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_gravity_wrench_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_sensor_wrench_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_target_wrench_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_net_force_wrench_publisher;
 };
 
 }

cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp

@@ -113,6 +113,23 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   m_target_wrench.setZero();
   m_ft_sensor_wrench.setZero();
 
+  // Controller-internal state publishing
+  m_feedback_gravity_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_gravity_wrench", 3);
+
+  m_feedback_sensor_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_sensor_wrench", 3);
+
+  m_feedback_target_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_target_wrench", 3);
+
+  m_feedback_net_force_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_net_force_wrench", 3);
+
   // Connect dynamic reconfigure and overwrite the default values with values
   // on the parameter server. This is done automatically if parameters with
   // the according names exist.
@@ -185,10 +202,28 @@ computeForceError()
     target_wrench = m_target_wrench;
   }
 
+  ctrl::Vector6D sensor_wrench;
+  sensor_wrench = Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref);
+
+  ctrl::Vector6D gravity_wrench;
+  gravity_wrench = compensateGravity();
+
   // Superimpose target wrench and sensor wrench in base frame
-  return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref)
+  ctrl::Vector6D net_force_wrench;
+  net_force_wrench = sensor_wrench
     + target_wrench
-    + compensateGravity();
+    + gravity_wrench;
+
+  // Publish wrench state feedback
+  if (Base::m_publish_state_feedback)
+  {
+    publishStateWrenchFeedback(m_feedback_sensor_wrench_publisher, sensor_wrench);
+    publishStateWrenchFeedback(m_feedback_target_wrench_publisher, target_wrench);
+    publishStateWrenchFeedback(m_feedback_gravity_wrench_publisher, gravity_wrench);
+    publishStateWrenchFeedback(m_feedback_net_force_wrench_publisher, net_force_wrench);
+  }
+
+  return net_force_wrench;
 }
 
 template <class HardwareInterface>
@@ -295,6 +330,25 @@ signalTaringCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Respons
   return true;
 }
 
+template <class HardwareInterface>
+void CartesianForceController<HardwareInterface>::
+publishStateWrenchFeedback(realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>& rt_publisher,
+                           ctrl::Vector6D& wrench)
+{
+  if (rt_publisher->trylock()){
+    rt_publisher->msg_.header.stamp = ros::Time::now();
+    rt_publisher->msg_.header.frame_id = Base::m_robot_base_link;
+    rt_publisher->msg_.wrench.force.x = wrench[0];
+    rt_publisher->msg_.wrench.force.y = wrench[1];
+    rt_publisher->msg_.wrench.force.z = wrench[2];
+    rt_publisher->msg_.wrench.torque.x = wrench[3];
+    rt_publisher->msg_.wrench.torque.y = wrench[4];
+    rt_publisher->msg_.wrench.torque.z = wrench[5];
+
+    rt_publisher->unlockAndPublish();
+  }
+}
+
 template <class HardwareInterface>
 void CartesianForceController<HardwareInterface>::dynamicReconfigureCallback(Config& config,
                                                                              uint32_t level)