Implement dynamic reconfiguration for aruco_map (#448)

aruco_pose/CMakeLists.txt

@@ -119,6 +119,7 @@ generate_messages(
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/Detector.cfg
+  cfg/Map.cfg
 )
 
 ###################################

aruco_pose/cfg/Map.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+PACKAGE = "aruco_pose"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if map detection enabled", True)
+
+gen.add("map", str_t, 0, "full path for the map file")
+
+gen.add("image_axis", bool_t, 0, "debug image axis", default=True)
+
+exit(gen.generate(PACKAGE, "aruco_pose", "Map"))

aruco_pose/src/aruco_map.cpp

@@ -19,11 +19,13 @@
 #include <vector>
 #include <fstream>
 #include <algorithm>
+#include <memory>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
 #include <tf/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -41,6 +43,7 @@
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
+#include <aruco_pose/MapConfig.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
@@ -74,6 +77,9 @@ private:
 	tf2_ros::StaticTransformBroadcaster static_br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::MapConfig>> dyn_srv_;
+	bool enabled_ = true;
+	std::string type_;
 	visualization_msgs::MarkerArray vis_array_;
 	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
@@ -96,8 +102,7 @@ public:
 			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 
-		std::string type, map;
-		type = nh_priv_.param<std::string>("type", "map");
+		type_ = nh_priv_.param<std::string>("type", "map");
 		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
 		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -110,13 +115,13 @@ public:
 
 		// createStripLine();
 
-		if (type == "map") {
-			param(nh_priv_, "map", map);
-			loadMap(map);
-		} else if (type == "gridboard") {
+		if (type_ == "map") {
+			map_ = nh_priv_.param<std::string>("map" , "");
+			loadMap(map_);
+		} else if (type_ == "gridboard") {
 			createGridBoard(nh_priv_);
 		} else {
-			NODELET_FATAL("unknown type: %s", type.c_str());
+			NODELET_FATAL("unknown type: %s", type_.c_str());
 			ros::shutdown();
 		}
 
@@ -124,10 +129,7 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
-		publishMarkersFrames();
-		publishMarkers();
-		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
+		publishMap();
 
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
@@ -136,6 +138,12 @@ public:
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::MapConfig>>(nh_priv_);
+		dynamic_reconfigure::Server<aruco_pose::MapConfig>::CallbackType cb;
+
+		cb = std::bind(&ArucoMap::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("ready");
 	}
 
@@ -143,6 +151,9 @@ public:
 	              const sensor_msgs::CameraInfoConstPtr& cinfo,
 	              const aruco_pose::MarkerArrayConstPtr& markers)
 	{
+		if (!enabled_) return;
+		if (markers->markers.empty()) return; // map not loaded
+
 		int valid = 0;
 		int count = markers->markers.size();
 		std::vector<int> ids;
@@ -268,9 +279,17 @@ publish_debug:
 		std::ifstream f(filename);
 		std::string line;
 
+		clearMarkers();
+
+		if (map_ == "") {
+			NODELET_INFO("No map loaded");
+			return;
+		}
+
 		if (!f.good()) {
-			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
-			ros::shutdown();
+			NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
+			map_ = "";
+			return;
 		}
 
 		while (std::getline(f, line)) {
@@ -296,9 +315,10 @@ publish_debug:
 				s.putback(first);
 			} else {
 				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
-				NODELET_FATAL("Malformed input: %s", line.c_str());
-				ros::shutdown();
-				throw std::runtime_error("Malformed input");
+				NODELET_ERROR("Malformed input: %s", line.c_str());
+				map_ = "";
+				clearMarkers();
+				return;
 			}
 
 			if (!(s >> id >> length >> x >> y)) {
@@ -329,6 +349,14 @@ publish_debug:
 		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}
 
+	void publishMap()
+	{
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+	}
+
 	void createGridBoard(ros::NodeHandle& nh)
 	{
 		NODELET_INFO("generate gridboard");
@@ -370,6 +398,15 @@ publish_debug:
 		}
 	}
 
+	void clearMarkers()
+	{
+		board_->ids.clear();
+		board_->objPoints.clear();
+		markers_.markers.clear();
+		vis_array_.markers.clear();
+		markers_transforms_.clear();
+	}
+
 	// void createStripLine()
 	// {
 	// 	visualization_msgs::Marker marker;
@@ -509,6 +546,22 @@ publish_debug:
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}
+
+	void paramCallback(aruco_pose::MapConfig &config, uint32_t level)
+	{
+		// https://github.com/CopterExpress/clover/commit/2cd334c474e3ed04ef65ca1ba7f08ab535a3dc6d#diff-942723f9452c398ae93f1a91427f9a7b614be5e5871f8a3e590f324d804f0d58R356
+		enabled_ = config.enabled;
+		if (type_ == "map" && config.map != map_) {
+			map_ = config.map;
+			loadMap(map_);
+			publishMap();
+		}
+
+		if (config.image_axis != image_axis_) {
+			image_axis_ = config.image_axis;
+			publishMapImage();
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/test/test_node_failure.py

@@ -2,6 +2,7 @@ import rospy
 import pytest
 
 from visualization_msgs.msg import MarkerArray as VisMarkerArray
+from aruco_pose.msg import MarkerArray
 
 
 @pytest.fixture
@@ -9,5 +10,5 @@ def node():
     return rospy.init_node('aruco_pose_test', anonymous=True)
 
 def test_node_failure(node):
-    with pytest.raises(rospy.exceptions.ROSException):
-        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5).markers == []
+    assert rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5).markers == []