From 5b9dd3ae61741504aa80af6475fe6c9d0ff4db26 Mon Sep 17 00:00:00 2001
From: David Watkins <djrival7@gmail.com>
Date: Thu, 23 Mar 2017 01:47:21 -0400
Subject: [PATCH 1/2] Added frame_prefix to parameters

---
 nodes/IndividualMarkers.cpp | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/nodes/IndividualMarkers.cpp b/nodes/IndividualMarkers.cpp
index a44a23f..32e2825 100644
--- a/nodes/IndividualMarkers.cpp
+++ b/nodes/IndividualMarkers.cpp
@@ -83,6 +83,7 @@ double max_track_error;
 std::string cam_image_topic;
 std::string cam_info_topic;
 std::string output_frame;
+std::string frame_prefix;
 int marker_resolution = 5; // default marker resolution
 int marker_margin = 2; // default marker margin
 
@@ -392,7 +393,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
       tf::Transform markerPose = t * m; // marker pose in the camera frame
 
 	  //Publish the transform from the camera to the marker
-	  std::string markerFrame = "ar_marker_";
+	  std::string markerFrame = frame_prefix + "ar_marker_";
 	  std::stringstream out;
 	  out << id;
 	  std::string id_string = out.str();
@@ -521,6 +522,8 @@ int main(int argc, char *argv[])
       marker_resolution = atoi(argv[8]);
     if (argc > 9)
       marker_margin = atoi(argv[9]);
+    if (argc > 10)
+      frame_prefix = argv[10];
 
   } else {
     // Get params from ros param server.
@@ -544,6 +547,13 @@ int main(int argc, char *argv[])
     cam_info_topic = "camera_info";
   }
 
+  if(frame_prefix == "" && pn.param("frame_prefix", frame_prefix) == "") {
+    ROS_ERROR("Param 'frame_prefix' has to be set to prevent ar conflicts");
+    exit(EXIT_FAILURE);
+  } else {
+    ROS_INFO("frame_prefix is '%s'", frame_prefix.c_str());
+  }
+
   // Set dynamically configurable parameters so they don't get replaced by default values
   pn.setParam("marker_size", marker_size);
   pn.setParam("max_new_marker_error", max_new_marker_error);
@@ -577,7 +587,7 @@ int main(int argc, char *argv[])
   if (enabled == true)
   {
     // This always happens, as enable is true by default
-    ROS_INFO("Subscribing to image topic");
+    ROS_INFO("Subscribing to image topic: %s", cam_image_topic.c_str());
     cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback);
   }
 

From b46763e962bf1ac8feb48f218b6c544edd9f4f96 Mon Sep 17 00:00:00 2001
From: David Watkins <djrival7@gmail.com>
Date: Thu, 23 Mar 2017 01:58:10 -0400
Subject: [PATCH 2/2] Fixed bug where param was not being properly retrieved

---
 nodes/IndividualMarkers.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nodes/IndividualMarkers.cpp b/nodes/IndividualMarkers.cpp
index 32e2825..ffc5d04 100644
--- a/nodes/IndividualMarkers.cpp
+++ b/nodes/IndividualMarkers.cpp
@@ -547,7 +547,7 @@ int main(int argc, char *argv[])
     cam_info_topic = "camera_info";
   }
 
-  if(frame_prefix == "" && pn.param("frame_prefix", frame_prefix) == "") {
+  if(frame_prefix == "" && !pn.getParam("frame_prefix", frame_prefix)) {
     ROS_ERROR("Param 'frame_prefix' has to be set to prevent ar conflicts");
     exit(EXIT_FAILURE);
   } else {