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

aruco_map: possibility to publish static transforms for map's markers

Browse Source
This commit is contained in:
Oleg Kalachev
2019-07-03 05:18:44 +03:00
parent 2ae5ffe09f
commit 48b24a5fef
4 changed files with 44 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
aruco_pose/src/aruco_map.cpp
View File

@@ -27,6 +27,7 @@
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
@@ -68,11 +69,13 @@ private:
Mat camera_matrix_, dist_coeffs_;
geometry_msgs::TransformStamped transform_;
geometry_msgs::PoseWithCovarianceStamped pose_;
vector<geometry_msgs::TransformStamped> markers_transforms_;
tf2_ros::TransformBroadcaster br_;
tf2_ros::StaticTransformBroadcaster static_br_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};
visualization_msgs::MarkerArray vis_array_;
std::string known_tilt_;
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
int image_width_, image_height_, image_margin_;
bool auto_flip_;
@@ -125,6 +128,7 @@ 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));
publishMarkersFrames();
publishMapImage();
vis_markers_pub_.publish(vis_array_);
@@ -422,6 +426,15 @@ publish_debug:
board_->ids.push_back(id);
board_->objPoints.push_back(obj_points);
// Add marker's static transform
if (!markers_frame_.empty()) {
geometry_msgs::TransformStamped marker_transform;
marker_transform.header.frame_id = markers_parent_frame_;
marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
tf::transformTFToMsg(transform, marker_transform.transform);
markers_transforms_.push_back(marker_transform);
}
// Add visualization marker
visualization_msgs::Marker marker;
marker.header.frame_id = transform_.child_frame_id;
@@ -452,6 +465,13 @@ publish_debug:
// vis_array_.markers.at(0).points.push_back(p);
}
void publishMarkersFrames()
{
if (!markers_transforms_.empty()) {
static_br_.sendTransform(markers_transforms_);
}
}
void publishMapImage()
{
cv::Size size(image_width_, image_height_);