Skip to content

Added support for tf_prefix #39

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
guihomework wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open

Added support for tf_prefix #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions tactile_merger/include/tactile_merger/merger.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ class Merger
{
public:
Merger();
explicit Merger(const std::string &tf_prefix = "");
~Merger();

void init(const std::string &param = "robot_description");
Expand All @@ -50,6 +51,7 @@ class Merger
tactile_msgs::TactileContacts getAllTaxelContacts();

private:
std::string tf_prefix_;
struct GroupData;
typedef boost::shared_ptr<GroupData> GroupDataPtr;

Expand Down
16 changes: 13 additions & 3 deletions tactile_merger/src/merger.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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);
Expand All @@ -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 "
Expand Down
4 changes: 3 additions & 1 deletion tactile_merger/src/merger_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_msgs::TactileContacts>("tactile_contact_states", 5);
Expand All @@ -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();
Expand Down