From e601080a957d3cd670ee1e66a2c64e2c4574616a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 21 Mar 2019 20:55:22 +0300 Subject: [PATCH] aruco_pose: add param auto_flip --- aruco_pose/src/aruco_detect.cpp | 6 ++++-- aruco_pose/src/aruco_map.cpp | 4 +++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 7a385eb5..d41d612b 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -62,7 +62,7 @@ private: image_transport::Publisher debug_pub_; image_transport::CameraSubscriber img_sub_; ros::Publisher markers_pub_, vis_markers_pub_; - bool estimate_poses_, send_tf_; + bool estimate_poses_, send_tf_, auto_flip_; double length_; std::unordered_map length_override_; std::string frame_id_prefix_, known_tilt_; @@ -87,6 +87,8 @@ public: readLengthOverride(); nh_priv_.param("known_tilt", known_tilt_, ""); + nh_priv_.param("auto_flip", auto_flip_, false); + nh_priv_.param("frame_id_prefix", frame_id_prefix_, "aruco_"); camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); @@ -177,7 +179,7 @@ private: // snap orientation (if enabled and snap frame available) if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) { - snapOrientation(marker.pose.orientation, snap_to.transform.rotation); + snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_); } // TODO: check IDs are unique diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index f5810117..5859b516 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -73,6 +73,7 @@ private: visualization_msgs::MarkerArray vis_array_; std::string known_tilt_; int image_width_, image_height_, image_margin_; + bool auto_flip_; public: virtual void onInit() @@ -95,6 +96,7 @@ public: nh_priv_.param("type", type, "map"); nh_priv_.param("frame_id", transform_.child_frame_id, "aruco_map"); nh_priv_.param("known_tilt", known_tilt_, ""); + nh_priv_.param("auto_flip", auto_flip_, false); nh_priv_.param("image_width", image_width_, 2000); nh_priv_.param("image_height", image_height_, 2000); nh_priv_.param("image_margin", image_margin_, 200); @@ -183,7 +185,7 @@ public: try { geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id, known_tilt_, markers->header.stamp, ros::Duration(0.02)); - snapOrientation(transform_.transform.rotation, snap_to.transform.rotation); + snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_); } catch (const tf2::TransformException& e) { ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what()); }