From e9305ec28013db989eb172e723f5313c1fbc74e2 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 9 Sep 2020 18:40:58 +0200
Subject: [PATCH] only compute if any subscriber
---
src/publisher_pose.cpp | 67 +++++++++++++++++++++++-------------------
1 file changed, 36 insertions(+), 31 deletions(-)
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index 8d2380c..5750bd0 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -15,17 +15,23 @@ PublisherPose::PublisherPose(const std::string& _unique_name,
const ProblemPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
- pose_array_ = _server.getParam<bool>(prefix_ + "/pose_array_msg");
- marker_ = _server.getParam<bool>(prefix_ + "/marker_msg");
- pose_with_cov_ = _server.getParam<bool>(prefix_ + "/pose_with_cov_msg");
- if (marker_)
- {
+ try{
+ std::cout << "PublisherPose: taking user defined marker color...\n";
Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color");
marker_color_.r = col(0);
marker_color_.g = col(1);
marker_color_.b = col(2);
marker_color_.a = col(3);
}
+ catch(...)
+ {
+ std::cout << "PublisherPose: using default marker color: RED\n";
+ marker_color_.r = 1;
+ marker_color_.g = 0;
+ marker_color_.b = 0;
+ marker_color_.a = 1;
+ }
+
extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
if (extrinsics_)
sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
@@ -38,35 +44,34 @@ void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
nh.param<std::string>("map_frame_id", map_frame_id_, "map");
// initialize msg and publisher
- if (pose_array_)
- {
- pose_array_msg_.header.frame_id = frame_id_;
- pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_pose_array", 1);
- }
- if (marker_)
- {
- marker_msg_.header.frame_id = frame_id_;
- marker_msg_.type = visualization_msgs::Marker::LINE_STRIP;
- marker_msg_.action = visualization_msgs::Marker::ADD;
- marker_msg_.ns = "trajectory";
- marker_msg_.scale.x = 0.1;
- marker_msg_.color = marker_color_;
- marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
-
- pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
- }
- if (pose_with_cov_)
- {
- pose_with_cov_msg_.header.frame_id = frame_id_;
+ // POSE ARRAY
+ pose_array_msg_.header.frame_id = frame_id_;
- pub_pose_with_cov_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic + "_pose_with_cov", 1);
- }
+ pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_pose_array", 1);
+
+ // MARKER
+ marker_msg_.header.frame_id = frame_id_;
+ marker_msg_.type = visualization_msgs::Marker::LINE_STRIP;
+ marker_msg_.action = visualization_msgs::Marker::ADD;
+ marker_msg_.ns = "trajectory";
+ marker_msg_.scale.x = 0.1;
+ marker_msg_.color = marker_color_;
+ marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+
+ pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
+
+ // POSE WITH COV
+ pose_with_cov_msg_.header.frame_id = frame_id_;
+
+ pub_pose_with_cov_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic + "_pose_with_cov", 1);
}
void PublisherPose::publishDerived()
{
- if (not pose_array_ and not marker_ and not pose_with_cov_)
+ if (pub_pose_array_.getNumSubscribers() == 0 and
+ pub_marker_.getNumSubscribers() == 0 and
+ pub_pose_with_cov_.getNumSubscribers() == 0 )
return;
VectorComposite current_state = problem_->getState("PO");
@@ -166,21 +171,21 @@ void PublisherPose::publishDerived()
void PublisherPose::publishPose()
{
// fill msgs and publish
- if (pose_array_)
+ if (pub_pose_array_.getNumSubscribers() != 0)
{
pose_array_msg_.header.stamp = pose_with_cov_msg_.header.stamp;
pose_array_msg_.poses.push_back(pose_with_cov_msg_.pose.pose);
pub_pose_array_.publish(pose_array_msg_);
}
- if (marker_)
+ if (pub_marker_.getNumSubscribers() != 0)
{
marker_msg_.header.stamp = pose_with_cov_msg_.header.stamp;
marker_msg_.points.push_back(pose_with_cov_msg_.pose.pose.position);
pub_marker_.publish(marker_msg_);
}
- if (pose_with_cov_)
+ if (pub_pose_with_cov_.getNumSubscribers() != 0)
{
pub_pose_with_cov_.publish(pose_with_cov_msg_);
}
--
GitLab