Implemented tf prefix for multiple robot / multiple camera systems

ar_track_alvar/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 tf_prefix_ar;
 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 = tf_prefix_ar + "ar_marker_";
 	  std::stringstream out;
 	  out << id;
 	  std::string id_string = out.str();
@@ -494,7 +495,7 @@ int main(int argc, char *argv[])
   ros::NodeHandle n, pn("~");
 
   if(argc > 1) {
-    ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings.");
+    ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings. (ar_track_alvar)");
 
     if(argc < 7){
       std::cout << std::endl;
@@ -532,6 +533,7 @@ int main(int argc, char *argv[])
     pn.param("marker_resolution", marker_resolution, 5);
     pn.param("marker_margin", marker_margin, 2);
     pn.param("output_frame_from_msg", output_frame_from_msg, false);
+	pn.param("tf_prefix_ar", tf_prefix_ar);
 
     if (!output_frame_from_msg && !pn.getParam("output_frame", output_frame)) {
       ROS_ERROR("Param 'output_frame' has to be set if the output frame is not "

ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp

@@ -72,6 +72,7 @@ double max_track_error;
 std::string cam_image_topic;
 std::string cam_info_topic;
 std::string output_frame;
+std::string tf_prefix;
 int marker_resolution = 5; // default marker resolution
 int marker_margin = 2; // default marker margin
 
@@ -134,7 +135,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
                 }
 
 				//Publish the transform from the camera to the marker
-				std::string markerFrame = "ar_marker_";
+                std::string markerFrame = tf_prefix + "/ar_marker_";
 				std::stringstream out;
 				out << id;
 				std::string id_string = out.str();
@@ -240,6 +241,7 @@ int main(int argc, char *argv[])
 	ros::init (argc, argv, "marker_detect");
 	ros::NodeHandle n, pn("~");
 
+	pn.getParam("tf_prefix", tf_prefix);
   if(argc > 1) {
     ROS_WARN("Command line arguments are deprecated. Consider using ROS parameters and remappings.");
 
@@ -278,6 +280,8 @@ int main(int argc, char *argv[])
     pn.setParam("max_frequency", max_frequency);  // in case it was not set.
     pn.param("marker_resolution", marker_resolution, 5);
     pn.param("marker_margin", marker_margin, 2);
+
+
     if (!pn.getParam("output_frame", output_frame)) {
       ROS_ERROR("Param 'output_frame' has to be set.");
       exit(EXIT_FAILURE);
@@ -293,6 +297,7 @@ int main(int argc, char *argv[])
   pn.setParam("max_new_marker_error", max_new_marker_error);
   pn.setParam("max_track_error", max_track_error);
 
+
 	marker_detector.SetMarkerSize(marker_size, marker_resolution, marker_margin);
 
 	cam = new Camera(n, cam_info_topic);