From 42f9b55f76f9d2fcc273c2bfc3b503b08e4c2b37 Mon Sep 17 00:00:00 2001 From: Your Name Date: Tue, 16 May 2017 19:21:21 +0200 Subject: [PATCH 1/3] Implemented tf prefix for multiple robot systems --- ar_track_alvar/cfg/Params.cfg | 1 + ar_track_alvar/nodes/IndividualMarkers.cpp | 6 ++++-- ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp | 6 +++++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/ar_track_alvar/cfg/Params.cfg b/ar_track_alvar/cfg/Params.cfg index 8903d41..d1bdbd4 100755 --- a/ar_track_alvar/cfg/Params.cfg +++ b/ar_track_alvar/cfg/Params.cfg @@ -11,6 +11,7 @@ gen.add("max_frequency", double_t, 0, "Maximum processing rate; frames coming at gen.add("marker_size", double_t, 0, "The width in centimeters of one side of the black square marker border", 10.0, 1.0, 100.0) gen.add("max_new_marker_error", double_t, 0, "A threshold determining when new markers can be detected under uncertainty", 0.08, 0.0, 2.0) gen.add("max_track_error", double_t, 0, "A threshold determining how much tracking error can be observed before an tag is considered to have disappeared", 0.2, 0.0, 4.0) +gen.add("tf_prefix_ar", str_t, 0, "Tf prefix for use with multiple robots", "") # Second arg is node name it will run in (doc purposes only), third is generated filename prefix exit(gen.generate(PACKAGE, "ar_track_alvar_configure", "Params")) diff --git a/ar_track_alvar/nodes/IndividualMarkers.cpp b/ar_track_alvar/nodes/IndividualMarkers.cpp index a44a23f..135cad4 100644 --- a/ar_track_alvar/nodes/IndividualMarkers.cpp +++ b/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 " diff --git a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp index d5a439e..eb187e3 100644 --- a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp +++ b/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_ar; 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 + "ar_marker_"; std::stringstream out; out << id; std::string id_string = out.str(); @@ -278,6 +279,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); + pn.param("tf_prefix_ar", tf_prefix_ar); + if (!pn.getParam("output_frame", output_frame)) { ROS_ERROR("Param 'output_frame' has to be set."); exit(EXIT_FAILURE); @@ -293,6 +296,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); From bf8c1d1589c3b7670c069887f83a37ec0c536efb Mon Sep 17 00:00:00 2001 From: Your Name Date: Tue, 16 May 2017 22:23:35 +0200 Subject: [PATCH 2/3] tf prefix update --- ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp index eb187e3..0cb973e 100644 --- a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp +++ b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp @@ -228,6 +228,7 @@ void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level) marker_size = config.marker_size; max_new_marker_error = config.max_new_marker_error; max_track_error = config.max_track_error; + tf_prefix_ar = config.tf_prefix_ar; } void enableCallback(const std_msgs::BoolConstPtr& msg) From 46e3cba90d849288ab6d89a6d025650a45187b5e Mon Sep 17 00:00:00 2001 From: Jakob Ludwiger Date: Tue, 25 Jul 2017 13:39:09 +0900 Subject: [PATCH 3/3] fixed tf_prefix --- ar_track_alvar/cfg/Params.cfg | 1 - ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp | 8 ++++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ar_track_alvar/cfg/Params.cfg b/ar_track_alvar/cfg/Params.cfg index d1bdbd4..8903d41 100755 --- a/ar_track_alvar/cfg/Params.cfg +++ b/ar_track_alvar/cfg/Params.cfg @@ -11,7 +11,6 @@ gen.add("max_frequency", double_t, 0, "Maximum processing rate; frames coming at gen.add("marker_size", double_t, 0, "The width in centimeters of one side of the black square marker border", 10.0, 1.0, 100.0) gen.add("max_new_marker_error", double_t, 0, "A threshold determining when new markers can be detected under uncertainty", 0.08, 0.0, 2.0) gen.add("max_track_error", double_t, 0, "A threshold determining how much tracking error can be observed before an tag is considered to have disappeared", 0.2, 0.0, 4.0) -gen.add("tf_prefix_ar", str_t, 0, "Tf prefix for use with multiple robots", "") # Second arg is node name it will run in (doc purposes only), third is generated filename prefix exit(gen.generate(PACKAGE, "ar_track_alvar_configure", "Params")) diff --git a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp index 0cb973e..2d6e48b 100644 --- a/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp +++ b/ar_track_alvar/nodes/IndividualMarkersNoKinect.cpp @@ -72,7 +72,7 @@ double max_track_error; std::string cam_image_topic; std::string cam_info_topic; std::string output_frame; -std::string tf_prefix_ar; +std::string tf_prefix; int marker_resolution = 5; // default marker resolution int marker_margin = 2; // default marker margin @@ -135,7 +135,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg) } //Publish the transform from the camera to the marker - std::string markerFrame = tf_prefix_ar + "ar_marker_"; + std::string markerFrame = tf_prefix + "/ar_marker_"; std::stringstream out; out << id; std::string id_string = out.str(); @@ -228,7 +228,6 @@ void configCallback(ar_track_alvar::ParamsConfig &config, uint32_t level) marker_size = config.marker_size; max_new_marker_error = config.max_new_marker_error; max_track_error = config.max_track_error; - tf_prefix_ar = config.tf_prefix_ar; } void enableCallback(const std_msgs::BoolConstPtr& msg) @@ -242,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."); @@ -280,7 +280,7 @@ 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); - pn.param("tf_prefix_ar", tf_prefix_ar); + if (!pn.getParam("output_frame", output_frame)) { ROS_ERROR("Param 'output_frame' has to be set.");