From 79479ce9825ae4ced1b320e60e9515fafc48830f Mon Sep 17 00:00:00 2001 From: qlin1806 Date: Sat, 20 Jul 2024 16:06:49 -0700 Subject: [PATCH] minor uptimizeation --- CMakeLists.txt | 8 +++- include/sas_robot_driver_franka.h | 7 +++- package.xml | 6 +++ .../sas_robot_dynamic_provider.cpp | 41 ++++++++++++++++--- .../sas_robot_dynamic_provider.h | 20 +++++++-- src/joint/sas_robot_driver_franka.cpp | 2 +- src/sas_robot_driver_franka_node.cpp | 2 +- 7 files changed, 70 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 277d16a..1f319f4 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index ef43158..ad27e2d 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -44,6 +44,7 @@ #include "robot_interface_franka.h" #include #include "sas_robot_dynamic_provider.h" +#include using namespace DQ_robotics; using namespace Eigen; @@ -70,7 +71,7 @@ private: std::shared_ptr 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 ); diff --git a/package.xml b/package.xml index def03b9..e77d4bc 100644 --- a/package.xml +++ b/package.xml @@ -52,6 +52,8 @@ roscpp rospy + tf2_ros + tf2 std_msgs sas_clock sas_robot_driver @@ -60,6 +62,8 @@ roscpp rospy + tf2_ros + tf2 std_msgs sas_clock sas_robot_driver @@ -69,6 +73,8 @@ roscpp rospy + tf2_ros + tf2 std_msgs sas_clock sas_robot_driver diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.cpp b/src/dynamic_interface/sas_robot_dynamic_provider.cpp index 30c0bd9..d1e1190 100644 --- a/src/dynamic_interface/sas_robot_dynamic_provider.cpp +++ b/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(node_prefix + "/get/cartesian_stiffness", 1); - publisher_stiffness_pose_ = publisher_nodehandle.advertise(node_prefix + "/get/stiffness_pose", 1); + // publisher_stiffness_pose_ = publisher_nodehandle.advertise(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); diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.h b/src/dynamic_interface/sas_robot_dynamic_provider.h index 81c030c..9f6db5b 100644 --- a/src/dynamic_interface/sas_robot_dynamic_provider.h +++ b/src/dynamic_interface/sas_robot_dynamic_provider.h @@ -33,25 +33,37 @@ #include #include #include -#include +#include +#include +#include +#include +#include +#include #include -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); }; diff --git a/src/joint/sas_robot_driver_franka.cpp b/src/joint/sas_robot_driver_franka.cpp index 6a4a0e7..1f0488d 100644 --- a/src/joint/sas_robot_driver_franka.cpp +++ b/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), diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index 0207796..94ef0d3 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/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