From 9fe9ff1d0acfc32d0682c130cc7e33e951dcc0b3 Mon Sep 17 00:00:00 2001
From: Lisa <robbie@uni-koblenz.de>
Date: Tue, 21 Mar 2017 14:40:53 +0100
Subject: [PATCH] lazy pose array publisher
---
homer_mapping/src/slam_node.cpp | 33 ++++++++++++++++++---------------
1 file changed, 18 insertions(+), 15 deletions(-)
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 05994a4a..ae17d3b9 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -149,23 +149,26 @@ void SlamNode::sendTfAndPose(Pose pose, ros::Time stamp)
void SlamNode::sendPoseArray(std::vector<Pose> poses)
{
- // Pose Array publishing
- geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray();
- poseArray.header.stamp = ros::Time::now();
- poseArray.header.frame_id = "/map";
-
- for (int i = 0; i < poses.size(); i++)
+ if (m_PoseArrayPublisher.getNumSubscribers() > 0)
{
- geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
- tmpPose.position.x = poses.at(i).x();
- tmpPose.position.y = poses.at(i).y();
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(poses.at(i).theta());
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg);
- tmpPose.orientation = quatMsg;
- poseArray.poses.push_back(tmpPose);
+ // Pose Array publishing
+ geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray();
+ poseArray.header.stamp = ros::Time::now();
+ poseArray.header.frame_id = "/map";
+
+ for (int i = 0; i < poses.size(); i++)
+ {
+ geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
+ tmpPose.position.x = poses.at(i).x();
+ tmpPose.position.y = poses.at(i).y();
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(poses.at(i).theta());
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg);
+ tmpPose.orientation = quatMsg;
+ poseArray.poses.push_back(tmpPose);
+ }
+ m_PoseArrayPublisher.publish(poseArray);
}
- m_PoseArrayPublisher.publish(poseArray);
}
void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
--
GitLab