diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 350a573c..38d19002 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -68,11 +69,13 @@ private: Mat camera_matrix_, dist_coeffs_; geometry_msgs::TransformStamped transform_; geometry_msgs::PoseWithCovarianceStamped pose_; + vector 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(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_); diff --git a/aruco_pose/test/basic.py b/aruco_pose/test/basic.py index 3be80838..9e146f99 100755 --- a/aruco_pose/test/basic.py +++ b/aruco_pose/test/basic.py @@ -78,6 +78,25 @@ def test_markers_frames(node, tf_buffer): assert marker_2.transform.rotation.z == approx(-0.107390951553) assert marker_2.transform.rotation.w == approx(0.0201999263402) +def test_map_markers_frames(node, tf_buffer): + stamp = rospy.get_rostime() + timeout = rospy.Duration(5) + + marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_1', stamp, timeout) + assert marker_1.transform.translation.x == approx(0) + assert marker_1.transform.translation.y == approx(0) + assert marker_1.transform.translation.z == approx(0) + + marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_4', stamp, timeout) + assert marker_4.transform.translation.x == approx(1) + assert marker_4.transform.translation.y == approx(1) + assert marker_4.transform.translation.z == approx(0) + + marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_12', stamp, timeout) + assert marker_12.transform.translation.x == approx(0.2) + assert marker_12.transform.translation.y == approx(0.5) + assert marker_12.transform.translation.z == approx(0) + def test_visualization(node): vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5) assert len(vis.markers) == 9 diff --git a/aruco_pose/test/basic.test b/aruco_pose/test/basic.test index 83bdd0c1..25853fd1 100644 --- a/aruco_pose/test/basic.test +++ b/aruco_pose/test/basic.test @@ -22,6 +22,8 @@ + + diff --git a/clever/launch/aruco.launch b/clever/launch/aruco.launch index bc8a3b86..5c508561 100644 --- a/clever/launch/aruco.launch +++ b/clever/launch/aruco.launch @@ -24,6 +24,8 @@ + +