diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index ce65c428..d1134eff 100644 --- a/aruco_pose/CMakeLists.txt +++ b/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 ) ################################### diff --git a/aruco_pose/cfg/Map.cfg b/aruco_pose/cfg/Map.cfg new file mode 100644 index 00000000..8c10bb36 --- /dev/null +++ b/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")) diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 02f32b0b..41250be8 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -19,11 +19,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -41,6 +43,7 @@ #include #include +#include #include #include @@ -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> 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(nh_priv_.param("dictionary", 2))); camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); - std::string type, map; - type = nh_priv_.param("type", "map"); + type_ = nh_priv_.param("type", "map"); transform_.child_frame_id = nh_priv_.param("frame_id", "aruco_map"); known_tilt_ = nh_priv_.param("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("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", 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(10), image_sub_, info_sub_, markers_sub_)); sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3)); + dyn_srv_ = std::make_shared>(nh_priv_); + dynamic_reconfigure::Server::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 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(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) diff --git a/aruco_pose/test/test_node_failure.py b/aruco_pose/test/test_node_failure.py index e0d8b2e9..cb28cfc7 100755 --- a/aruco_pose/test/test_node_failure.py +++ b/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 == [] diff --git a/docs/en/snippets.md b/docs/en/snippets.md index 1d0c3a41..87eade17 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -390,6 +390,26 @@ rospy.sleep(5) flow_client.update_configuration({'enabled': True}) ``` + + +### # {#aruco-map-dynamic} + +> **Info** For [RPi image](image.md) version > 0.23. + +Change the used [ArUco markers map file](aruco_map.md) dynamically: + + + +```python +import rospy +import dynamic_reconfigure.client + +rospy.init_node('flight') +map_client = dynamic_reconfigure.client.Client('aruco_map') + +map_client.update_configuration({'map': '/home/pi/catkin_ws/src/clover/aruco_pose/map/office.txt'}) +``` + ### # {#wait-global-position} Wait for global position to appear (finishing [GPS receiver](gps.md) initialization): diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index 7a9f2271..30bf38ce 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -401,6 +401,26 @@ rospy.sleep(5) flow_client.update_configuration({'enabled': True}) ``` + + +### # {#aruco-map-dynamic} + +> **Info** Для [образа](image.md) версии > 0.23. + +Динамически изменить используемый файл с [картой ArUco-маркеров](aruco_map.md): + + + +```python +import rospy +import dynamic_reconfigure.client + +rospy.init_node('flight') +map_client = dynamic_reconfigure.client.Client('aruco_map') + +map_client.update_configuration({'map': '/home/pi/catkin_ws/src/clover/aruco_pose/map/office.txt'}) +``` + ### # {#wait-global-position} Ожидать появления глобальной позиции (окончания инициализации [GPS-приемника](gps.md)):