From f5a3ce26e2e8a82c065e80ec0d7c18acb8964563 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Thu, 26 Jan 2023 21:10:11 +0100 Subject: [PATCH] Added support for tf_prefix --- tactile_merger/include/tactile_merger/merger.h | 2 ++ tactile_merger/src/merger.cpp | 16 +++++++++++++--- tactile_merger/src/merger_node.cpp | 4 +++- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/tactile_merger/include/tactile_merger/merger.h b/tactile_merger/include/tactile_merger/merger.h index 8c77e5e..7d1ee9d 100644 --- a/tactile_merger/include/tactile_merger/merger.h +++ b/tactile_merger/include/tactile_merger/merger.h @@ -38,6 +38,7 @@ class Merger { public: Merger(); + explicit Merger(const std::string &tf_prefix = ""); ~Merger(); void init(const std::string ¶m = "robot_description"); @@ -50,6 +51,7 @@ class Merger tactile_msgs::TactileContacts getAllTaxelContacts(); private: + std::string tf_prefix_; struct GroupData; typedef boost::shared_ptr GroupDataPtr; diff --git a/tactile_merger/src/merger.cpp b/tactile_merger/src/merger.cpp index f1eff42..1a8d886 100644 --- a/tactile_merger/src/merger.cpp +++ b/tactile_merger/src/merger.cpp @@ -46,7 +46,17 @@ struct Merger::GroupData Merger::GroupData::GroupData(const TaxelGroupPtr &group) : group(group) {} -Merger::Merger() {} +Merger::Merger(const std::string &tf_prefix) : tf_prefix_(tf_prefix) +{ + // prepare tf_prefix for easy future concatenation + if (!tf_prefix_.empty()) { + if (tf_prefix_[0] == '/') + tf_prefix_.erase(0, 1); // strip front slash if any + if (!tf_prefix_.empty() && tf_prefix_.back() != '/') + tf_prefix_.push_back('/'); // if still non-empty and not trailing slash, add it + } +} + Merger::~Merger() { sensors_.clear(); @@ -117,7 +127,7 @@ tactile_msgs::TactileContacts Merger::getAllTaxelContacts() tactile_msgs::TactileContact contact; contact.name = name_data.first; // group name - contact.header.frame_id = data->group->frame(); + contact.header.frame_id = tf_prefix_ + data->group->frame(); contact.header.stamp = data->timestamp; // insert all contacts of the group into result array data->group->all(contacts.contacts, contact); @@ -141,7 +151,7 @@ tactile_msgs::TactileContacts Merger::getGroupAveragedContacts() } tactile_msgs::TactileContact contact; contact.name = name_data.first; // group name - contact.header.frame_id = data->group->frame(); + contact.header.frame_id = tf_prefix_ + data->group->frame(); contact.header.stamp = data->timestamp; if (!data->group->average(contact)) { ROS_DEBUG_STREAM_NAMED("contacts", "getGroupAveragedContacts no contact for group of frame_id " diff --git a/tactile_merger/src/merger_node.cpp b/tactile_merger/src/merger_node.cpp index fcbedb9..d480799 100644 --- a/tactile_merger/src/merger_node.cpp +++ b/tactile_merger/src/merger_node.cpp @@ -48,7 +48,8 @@ int main(int argc, char *argv[]) ros::NodeHandle nh; ros::NodeHandle nh_priv("~"); - tactile::Merger merger; + std::string tf_prefix = nh_priv.param("tf_prefix", std::string()); + tactile::Merger merger(tf_prefix); merger.init(); ros::Publisher pub = nh.advertise("tactile_contact_states", 5); @@ -60,6 +61,7 @@ int main(int argc, char *argv[]) ros::Time last_update; ros::Rate rate(nh_priv.param("rate", 100.)); bool no_clustering = nh_priv.param("no_clustering", false); + while (ros::ok()) { ros::spinOnce(); ros::Time now = ros::Time::now();