From 1de3c2cbebb32a9406999c0df945ed00830ffde3 Mon Sep 17 00:00:00 2001
From: qlin1806 <qlin1806@g.ecc.u-tokyo.ac.jp>
Date: Sat, 20 Jul 2024 16:17:51 -0700
Subject: [PATCH] rate optimization

---
 .../sas_robot_dynamic_provider.cpp              | 17 +++++++++--------
 .../sas_robot_dynamic_provider.h                |  2 ++
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.cpp b/src/dynamic_interface/sas_robot_dynamic_provider.cpp
index d1e1190..0f29ac3 100644
--- a/src/dynamic_interface/sas_robot_dynamic_provider.cpp
+++ b/src/dynamic_interface/sas_robot_dynamic_provider.cpp
@@ -44,7 +44,6 @@ RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle
 {
     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::TransformStamped>(node_prefix + "/get/stiffness_pose", 1);
 
 }
 
@@ -71,13 +70,6 @@ void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const
     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 = header;
     msg.header.frame_id = child_frame_id_;
@@ -88,5 +80,14 @@ void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const
     msg.wrench.torque.y = torque(1);
     msg.wrench.torque.z = torque(2);
     publisher_cartesian_stiffness_.publish(msg);
+    if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
+    {
+        geometry_msgs::TransformStamped tf_msg;
+        tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
+        tf_msg.header = header;
+        tf_msg.header.frame_id = parent_frame_id_;
+        tf_msg.child_frame_id = child_frame_id_;
+        tf_broadcaster_.sendTransform(tf_msg);
+    }
 }
 
diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.h b/src/dynamic_interface/sas_robot_dynamic_provider.h
index 9f6db5b..cb63117 100644
--- a/src/dynamic_interface/sas_robot_dynamic_provider.h
+++ b/src/dynamic_interface/sas_robot_dynamic_provider.h
@@ -41,6 +41,8 @@
 #include <std_msgs/Header.h>
 #include <dqrobotics/DQ.h>
 
+#define REDUCE_TF_PUBLISH_RATE 10
+
 namespace qros {
 
 using namespace DQ_robotics;