CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_detect: implement ~/set_length_override service

Browse Source 
For changing individual markers length dynamically
This commit is contained in:
Oleg Kalachev
2022-05-24 05:15:53 +03:00
parent d0ab69df7f
commit 031c8b5305
2 changed files with 34 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
aruco_pose/src/aruco_detect.cpp
View File

@@ -48,6 +48,7 @@
#include <aruco_pose/Marker.h>
#include <aruco_pose/MarkerArray.h>
#include <aruco_pose/DetectorConfig.h>
#include <aruco_pose/SetMarkers.h>
#include "utils.h"
#include <memory>
@@ -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<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(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<aruco_pose::MarkerArray>("markers", 1);
vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("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();

7
aruco_pose/srv/SetMarkers.srv Normal file
View File

@@ -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