minor uptimizeation

CMakeLists.txt

@@ -19,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  tf2_ros
+  tf2
   sas_common
   sas_clock
   sas_robot_driver
@@ -27,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_package(
   INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver
+  CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2
 )
 
 find_package(Franka REQUIRED)
@@ -101,7 +103,9 @@ include_directories(
 
 
 add_library(sas_robot_dynamic_provider src/dynamic_interface/sas_robot_dynamic_provider.cpp)
-target_link_libraries(sas_robot_dynamic_provider dqrobotics)
+target_link_libraries(sas_robot_dynamic_provider
+        ${catkin_LIBRARIES}
+        dqrobotics)
 
 
 add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)

include/sas_robot_driver_franka.h

@@ -44,6 +44,7 @@
 #include "robot_interface_franka.h"
 #include <ros/common.h>
 #include "sas_robot_dynamic_provider.h"
+#include <thread>
 
 using namespace DQ_robotics;
 using namespace Eigen;
@@ -70,7 +71,7 @@ private:
 
     std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
 
-    RobotDynamicProvider* robot_dynamic_provider_;
+    qros::RobotDynamicProvider* robot_dynamic_provider_;
 
     //Joint positions
     VectorXd joint_positions_;
@@ -86,6 +87,8 @@ private:
     void _update_stiffness_contact_and_pose() const;
 
 
+
+
 public:
     //const static int SLAVE_MODE_JOINT_CONTROL;
     //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
@@ -95,7 +98,7 @@ public:
     ~RobotDriverFranka();
 
     RobotDriverFranka(
-        RobotDynamicProvider* robot_dynamic_provider,
+        qros::RobotDynamicProvider* robot_dynamic_provider,
         const RobotDriverFrankaConfiguration& configuration,
         std::atomic_bool* break_loops
         );

package.xml

@@ -52,6 +52,8 @@
 
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>sas_clock</build_depend>
   <build_depend>sas_robot_driver</build_depend>
@@ -60,6 +62,8 @@
 
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>sas_clock</build_export_depend>
   <build_export_depend>sas_robot_driver</build_export_depend>
@@ -69,6 +73,8 @@
 
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>tf2</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>sas_clock</exec_depend>
   <exec_depend>sas_robot_driver</exec_depend>

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