CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-30 06:49:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_pose: don’t publish transforms of markers in the map

Browse Source
This commit is contained in:
Oleg Kalachev
2019-09-04 19:14:55 +03:00
parent 4b0605fcbb
commit 8696ad14a5
2 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
aruco_pose/src/aruco_detect.cpp
View File

@@ -19,6 +19,7 @@
#include <string>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
@@ -62,12 +63,14 @@ private:
image_transport::Publisher debug_pub_;
image_transport::CameraSubscriber img_sub_;
ros::Publisher markers_pub_, vis_markers_pub_;
ros::Subscriber map_markers_sub_;
bool estimate_poses_, send_tf_, auto_flip_;
double length_;
std::unordered_map<int, double> length_override_;
std::string frame_id_prefix_, known_tilt_;
Mat camera_matrix_, dist_coeffs_;
aruco_pose::MarkerArray array_;
std::unordered_set<int> map_markers_ids_;
visualization_msgs::MarkerArray vis_array_;
public:
@@ -101,6 +104,7 @@ public:
image_transport::ImageTransport it(nh_);
image_transport::ImageTransport it_priv(nh_priv_);
map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, 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);
@@ -186,8 +190,8 @@ private:
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
// check if such static transform exists
if (!tf_buffer_.canTransform(transform.header.frame_id, transform.child_frame_id, transform.header.stamp)) {
// check if such static transform is in the map
if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
@@ -329,6 +333,14 @@ private:
return length_;
}
}
void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
{
map_markers_ids_.clear();
for (auto const& marker : msg.markers) {
map_markers_ids_.insert(marker.id);
}
}
};
PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

1
clever/launch/aruco.launch
View File

@@ -9,6 +9,7 @@
<node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
<remap from="image_raw" to="main_camera/image_raw"/>
<remap from="camera_info" to="main_camera/camera_info"/>
<remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
<param name="estimate_poses" value="true"/>
<param name="send_tf" value="true"/>
<param name="known_tilt" value="map"/>