diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 26d27b0c..6da1e8f3 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include "utils.h" #include @@ -69,6 +70,7 @@ private: image_transport::CameraSubscriber img_sub_; ros::Publisher markers_pub_, vis_markers_pub_; ros::Subscriber map_markers_sub_; + ros::ServiceServer set_markers_srv_; bool estimate_poses_, send_tf_, auto_flip_; double length_; ros::Duration transform_timeout_; @@ -116,6 +118,8 @@ public: dyn_srv_ = std::make_shared>(nh_priv_); dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2)); + set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, this); + debug_pub_ = it_priv.advertise("debug", 1); markers_pub_ = nh_priv_.advertise("markers", 1); vis_markers_pub_ = nh_priv_.advertise("visualization", 1); @@ -348,6 +352,29 @@ private: } } + bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res) + { + for (auto const& marker : req.markers) { + if (marker.id > 999) { + res.message = "Invalid marker id: " + std::to_string(marker.id); + ROS_ERROR("%s", res.message.c_str()); + return true; + } + if (!std::isfinite(marker.length) || marker.length <= 0) { + res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length); + ROS_ERROR("%s", res.message.c_str()); + return true; + } + } + + for (auto const& marker : req.markers) { + length_override_[marker.id] = marker.length; + } + + res.success = true; + return true; + } + void mapMarkersCallback(const aruco_pose::MarkerArray& msg) { map_markers_ids_.clear(); diff --git a/aruco_pose/srv/SetMarkers.srv b/aruco_pose/srv/SetMarkers.srv new file mode 100644 index 00000000..291aba8c --- /dev/null +++ b/aruco_pose/srv/SetMarkers.srv @@ -0,0 +1,7 @@ +# * Add or change markers in the map +# * Change markers' properties, e. g. lengths + +Marker[] markers # if length or pose is nan - remove from map +--- +bool success +string message