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_pose: add param auto_flip

Browse Source
This commit is contained in:
Oleg Kalachev
2019-03-21 20:55:22 +03:00
parent 84c16a7296
commit e601080a95
2 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
aruco_pose/src/aruco_detect.cpp
View File

@@ -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<int, double> length_override_;
std::string frame_id_prefix_, known_tilt_;
@@ -87,6 +87,8 @@ public:
readLengthOverride();
nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
nh_priv_.param("auto_flip", auto_flip_, false);
nh_priv_.param<std::string>("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

4
aruco_pose/src/aruco_map.cpp
View File

@@ -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<std::string>("type", type, "map");
nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
nh_priv_.param<std::string>("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());
}