qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

rate optimization

Browse Source
This commit is contained in:
qlin1806
2024-07-20 16:17:51 -07:00
parent 79479ce982
commit 1de3c2cbeb
2 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/dynamic_interface/sas_robot_dynamic_provider.cpp
View File

@@ -44,7 +44,6 @@ RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle
{ {
ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix); 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_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; std_msgs::Header header;
header.stamp = ros::Time::now(); header.stamp = ros::Time::now();
header.seq = seq_++; 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; geometry_msgs::WrenchStamped msg;
msg.header = header; msg.header = header;
msg.header.frame_id = child_frame_id_; 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.y = torque(1);
msg.wrench.torque.z = torque(2); msg.wrench.torque.z = torque(2);
publisher_cartesian_stiffness_.publish(msg); 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);
}
} }

2
src/dynamic_interface/sas_robot_dynamic_provider.h
View File

@@ -41,6 +41,8 @@
#include <std_msgs/Header.h> #include <std_msgs/Header.h>
#include <dqrobotics/DQ.h> #include <dqrobotics/DQ.h>
#define REDUCE_TF_PUBLISH_RATE 10
namespace qros { namespace qros {
using namespace DQ_robotics; using namespace DQ_robotics;