minor uptimizeation

src/dynamic_interface/sas_robot_dynamic_provider.cpp

@@ -29,7 +29,7 @@
 */
 #include "sas_robot_dynamic_provider.h"
 
-using namespace sas;
+using namespace qros;
 
 RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const std::string &node_prefix):
     RobotDynamicProvider(nodehandle, nodehandle, node_prefix)
@@ -38,20 +38,49 @@ RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const st
 }
 
 RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
-    node_prefix_(node_prefix)
+    node_prefix_(node_prefix),
+    child_frame_id_(node_prefix + "_stiffness_frame"),
+    parent_frame_id_(node_prefix + "_base")
 {
     ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix);
     publisher_cartesian_stiffness_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_stiffness", 1);
-    publisher_stiffness_pose_ = publisher_nodehandle.advertise<geometry_msgs::PoseStamped>(node_prefix + "/get/stiffness_pose", 1);
+    // publisher_stiffness_pose_ = publisher_nodehandle.advertise<geometry_msgs::TransformStamped>(node_prefix + "/get/stiffness_pose", 1);
 
 }
 
 
-void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const
+geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(const DQ& pose)
 {
-    publisher_stiffness_pose_.publish(dq_to_geometry_msgs_pose_stamped(base_to_stiffness));
+    geometry_msgs::Transform tf_msg;
+    const auto t = translation(pose);
+    const auto r = rotation(pose);
+    tf_msg.translation.x = t.q(1);
+    tf_msg.translation.y = t.q(2);
+    tf_msg.translation.z = t.q(3);
+    tf_msg.rotation.w = r.q(0);
+    tf_msg.rotation.x = r.q(1);
+    tf_msg.rotation.y = r.q(2);
+    tf_msg.rotation.z = r.q(3);
+    return tf_msg;
+}
+
+
+
+void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
+{
+    std_msgs::Header header;
+    header.stamp = ros::Time::now();
+    header.seq = seq_++;
+    header.frame_id = parent_frame_id_;
+    geometry_msgs::TransformStamped tf_msg;
+    tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
+    tf_msg.header = header;
+    tf_msg.child_frame_id = child_frame_id_;
+    tf_broadcaster_.sendTransform(tf_msg);
+    // publisher_stiffness_pose_.publish(pose_msg);
     geometry_msgs::WrenchStamped msg;
-    msg.header.stamp = ros::Time::now();
+    msg.header = header;
+    msg.header.frame_id = child_frame_id_;
     msg.wrench.force.x = force(0);
     msg.wrench.force.y = force(1);
     msg.wrench.force.z = force(2);

src/dynamic_interface/sas_robot_dynamic_provider.h

@@ -33,25 +33,37 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/sas_conversions.h>
 #include <geometry_msgs/WrenchStamped.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/Header.h>
 #include <dqrobotics/DQ.h>
 
-namespace sas {
+namespace qros {
 
 using namespace DQ_robotics;
 
 class RobotDynamicProvider {
 private:
+    unsigned int seq_ = 0;
+
     std::string node_prefix_;
+    std::string child_frame_id_;
+    std::string parent_frame_id_;
 
     ros::Publisher publisher_cartesian_stiffness_;
-    ros::Publisher publisher_stiffness_pose_;
+    tf2_ros::TransformBroadcaster tf_broadcaster_;
+
+
+    static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ;
 
 public:
     RobotDynamicProvider(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
     RobotDynamicProvider(ros::NodeHandle& publisher_nodehandle, ros::NodeHandle& subscriber_nodehandle, const std::string& node_prefix=ros::this_node::getName());
 
-    void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const;
+    void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque);
 
 };
 

src/joint/sas_robot_driver_franka.cpp

@@ -39,7 +39,7 @@
 
 namespace sas
 {
-    RobotDriverFranka::RobotDriverFranka(RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
+    RobotDriverFranka::RobotDriverFranka(qros::RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
     RobotDriver(break_loops),
     configuration_(configuration),
     robot_dynamic_provider_(robot_dynamic_provider),

src/sas_robot_driver_franka_node.cpp

@@ -133,7 +133,7 @@ int main(int argc, char **argv)
 
     // initialize the robot dynamic provider
     robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-    sas::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
+    qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
 
 
     try