automatic publishing if any subscriber

src/subscriber_gnss_fix_publisher_ecef.cpp

@@ -14,49 +14,46 @@ SubscriberGnssFixPublisherEcef::SubscriberGnssFixPublisherEcef(const std::string
     , ENU_defined_(false)
 {
     period_         = _server.getParam<double>(prefix_ + "/period");
-    pose_           = _server.getParam<bool>(prefix_ + "/pose_msg");
-    pose_array_     = _server.getParam<bool>(prefix_ + "/pose_array_msg");
-    marker_         = _server.getParam<bool>(prefix_ + "/marker_msg");
-    if (marker_)
-    {
+
+    // marker color
+    try{
+        std::cout << "SubscriberGnssFixPublisherEcef: 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 << "SubscriberGnssFixPublisherEcef: using default marker color: BLUE\n";
+        marker_color_.r = 0;
+        marker_color_.g = 0;
+        marker_color_.b = 1;
+        marker_color_.a = 1;
+    }
 }
 
-
 void SubscriberGnssFixPublisherEcef::initialize(ros::NodeHandle& nh, const std::string& topic)
 {
     sub_    = nh.subscribe(topic, 1, &SubscriberGnssFixPublisherEcef::callback, this);
 
-    // initialize msg and publisher
-    if (pose_)
-    {
-        pose_msg_.header.frame_id = "ENU";
+    // initialize msgs and publishers
+    pose_msg_.header.frame_id = "ENU";
+    pub_pose_ = nh.advertise<geometry_msgs::PoseStamped>(topic + "_ENU_pose", 1);
 
-        pub_pose_ = nh.advertise<geometry_msgs::PoseStamped>(topic + "_ENU_pose", 1);
-    }
-    if (pose_array_)
-    {
-        pose_array_msg_.header.frame_id = "ENU";
+    pose_array_msg_.header.frame_id = "ENU";
+    pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_ENU_pose_array", 1);
 
-        pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_ENU_pose_array", 1);
-    }
-    if (marker_)
-    {
-        marker_msg_.header.frame_id = "ENU";
-        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 + "_ENU_marker", 1);
-    }
+    marker_msg_.header.frame_id = "ENU";
+    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 + "_ENU_marker", 1);
 }
 
 void SubscriberGnssFixPublisherEcef::callback(const sensor_msgs::NavSatFix::ConstPtr& _msg)
@@ -110,21 +107,21 @@ bool SubscriberGnssFixPublisherEcef::listenTf()
 void SubscriberGnssFixPublisherEcef::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp)
 {
     // fill msgs and publish
-    if (pose_)
+    if (pub_pose_.getNumSubscribers() != 0)
     {
         pose_msg_.header.stamp = stamp;
         pose_msg_.pose = pose;
 
         pub_pose_.publish(pose_msg_);
     }
-    if (pose_array_)
+    if (pub_pose_array_.getNumSubscribers() != 0)
     {
         pose_array_msg_.header.stamp = stamp;
         pose_array_msg_.poses.push_back(pose);
 
         pub_pose_array_.publish(pose_array_msg_);
     }
-    if (marker_)
+    if (pub_marker_.getNumSubscribers() != 0)
     {
         marker_msg_.header.stamp = stamp;
         marker_msg_.points.push_back(pose.position);