diff --git a/aruco_pose/README.md b/aruco_pose/README.md index d5c88b1f..1e899068 100644 --- a/aruco_pose/README.md +++ b/aruco_pose/README.md @@ -43,7 +43,8 @@ It's recommended to run it within the same nodelet manager with the camera nodel * `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`) * `~length` (*double*) – markers' sides length * `~length_override` (*map*) – lengths of markers with specified ids -* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame +* `~known_vertical` (*string*) – known vertical (Z axis) of all the markers as a frame +* `~flip_vertical` – flip vertical vector ### Topics @@ -71,7 +72,8 @@ It's recommended to run it within the same nodelet manager with the camera nodel * `~map` – path to text file with markers list * `~frame_id` – published frame id (default: `aruco_map`) -* `~known_tilt` – known tilt (pitch and roll) of markers map as a frame +* `~known_vertical` – known vertical (Z axis) of markers map as a frame +* `~flip_vertical` – flip vertical vector * `~image_width` – debug image width (default: 2000) * `~image_height` – debug image height (default: 2000) * `~image_margin` – debug image margin (default: 200) diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 0e7803a7..8d3337c1 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -71,12 +71,12 @@ private: ros::Publisher markers_pub_, vis_markers_pub_; ros::Subscriber map_markers_sub_; ros::ServiceServer set_markers_srv_; - bool estimate_poses_, send_tf_, auto_flip_, use_map_markers_; + bool estimate_poses_, send_tf_, flip_vertical_, auto_flip_, use_map_markers_; bool waiting_for_map_; double length_; ros::Duration transform_timeout_; std::unordered_map length_override_; - std::string frame_id_prefix_, known_tilt_; + std::string frame_id_prefix_, known_vertical_; Mat camera_matrix_, dist_coeffs_; aruco_pose::MarkerArray array_; std::unordered_set map_markers_ids_; @@ -105,7 +105,8 @@ public: readLengthOverride(nh_priv_); transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02)); - known_tilt_ = nh_priv_.param("known_tilt", ""); + known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name + flip_vertical_ = nh_priv_.param("flip_vertical", false); auto_flip_ = nh_priv_.param("auto_flip", false); frame_id_prefix_ = nh_priv_.param("frame_id_prefix", "aruco_"); @@ -144,7 +145,7 @@ private: vector> corners, rejected; vector rvecs, tvecs; vector obj_points; - geometry_msgs::TransformStamped snap_to; + geometry_msgs::TransformStamped vertical; // Detect markers cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected); @@ -179,12 +180,12 @@ private: } } - if (!known_tilt_.empty()) { + if (!known_vertical_.empty()) { try { - snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_, - msg->header.stamp, transform_timeout_); + vertical = tf_buffer_->lookupTransform(msg->header.frame_id, known_vertical_, + msg->header.stamp, transform_timeout_); } catch (const tf2::TransformException& e) { - NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what()); + NODELET_WARN_THROTTLE(5, "can't retrieve known vertical: %s", e.what()); } } } @@ -205,9 +206,9 @@ private: if (estimate_poses_) { fillPose(marker.pose, rvecs[i], tvecs[i]); - // 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, auto_flip_); + // apply known vertical (if enabled and vertical frame available) + if (!known_vertical_.empty() && !vertical.header.frame_id.empty()) { + applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_); } if (send_tf_) { diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 3d59e875..10b717cc 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -81,9 +81,9 @@ private: bool enabled_ = true; std::string type_; visualization_msgs::MarkerArray vis_array_; - std::string known_tilt_, map_, markers_frame_, markers_parent_frame_; + std::string known_vertical_, map_, markers_frame_, markers_parent_frame_; int image_width_, image_height_, image_margin_; - bool auto_flip_, image_axis_; + bool flip_vertical_, auto_flip_, image_axis_; public: virtual void onInit() @@ -104,7 +104,8 @@ public: type_ = nh_priv_.param("type", "map"); transform_.child_frame_id = nh_priv_.param("frame_id", "aruco_map"); - known_tilt_ = nh_priv_.param("known_tilt", ""); + known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name + flip_vertical_ = nh_priv_.param("flip_vertical", false); auto_flip_ = nh_priv_.param("auto_flip", false); image_width_ = nh_priv_.param("image_width" , 2000); image_height_ = nh_priv_.param("image_height", 2000); @@ -177,7 +178,7 @@ public: corners.push_back(marker_corners); } - if (known_tilt_.empty()) { + if (known_vertical_.empty()) { // simple estimation valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_, rvec, tvec, false); @@ -191,7 +192,7 @@ public: } else { Mat obj_points, img_points; - // estimation with "snapping" + // estimation with known vertical cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points); if (obj_points.empty()) goto publish_debug; @@ -203,11 +204,11 @@ public: fillTransform(transform_.transform, rvec, tvec); 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, auto_flip_); + geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id, + known_vertical_, markers->header.stamp, ros::Duration(0.02)); + applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_); } catch (const tf2::TransformException& e) { - NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what()); + NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what()); } geometry_msgs::TransformStamped shift; diff --git a/aruco_pose/src/utils.h b/aruco_pose/src/utils.h index b7ae0432..cef041bf 100644 --- a/aruco_pose/src/utils.h +++ b/aruco_pose/src/utils.h @@ -106,26 +106,25 @@ inline bool isFlipped(tf::Quaternion& q) return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2); } -/* Set roll and pitch from "from" to "to", keeping yaw */ -inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false) +/* Apply a vertical to an orientation */ +inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical, + bool flip_vertical = false, bool auto_flip = false) // editorconfig-checker-disable-line { - tf::Quaternion _from, _to; - tf::quaternionMsgToTF(from, _from); - tf::quaternionMsgToTF(to, _to); + tf::Quaternion _vertical, _orientation; + tf::quaternionMsgToTF(vertical, _vertical); + tf::quaternionMsgToTF(orientation, _orientation); - if (auto_flip) { - if (!isFlipped(_from)) { - static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0); - _from *= flip; // flip "from" - } + if (flip_vertical || (auto_flip && !isFlipped(_orientation))) { + static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0); + _vertical *= flip; // flip vertical } - auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from)); + auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical)); double _, yaw; diff.getRPY(_, _, yaw); auto q = tf::createQuaternionFromRPY(0, 0, -yaw); - _from = _from * q; // set yaw from "to" to "from" - tf::quaternionTFToMsg(_from, to); // set "from" to "to" + _vertical = _vertical * q; // set yaw from orientation to vertical + tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation } inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose) diff --git a/clover/launch/aruco.launch b/clover/launch/aruco.launch index 4fbb5e44..0a540645 100644 --- a/clover/launch/aruco.launch +++ b/clover/launch/aruco.launch @@ -19,8 +19,8 @@ - - + + @@ -36,8 +36,8 @@ - - + + @@ -53,8 +53,4 @@ - - - diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index a465beb2..371dc8b5 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -401,12 +401,15 @@ def check_aruco(): info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length', '?')) except KeyError: failure('aruco_detect/length parameter is not set') - known_tilt = rospy.get_param('aruco_detect/known_tilt', '') - if known_tilt == 'map': - known_tilt += ' (ALL markers are on the floor)' - elif known_tilt == 'map_flipped': - known_tilt += ' (ALL markers are on the ceiling)' - info('aruco_detect/known_tilt = %s', known_tilt) + known_vertical = rospy.get_param('aruco_detect/known_vertical', '') + flip_vertical = rospy.get_param('aruco_detect/flip_vertical', False) + description = '' + if known_vertical == 'map' and not flip_vertical: + description = ' (all markers are on the floor)' + elif known_vertical == 'map' and flip_vertical: + description = ' (all markers are on the ceiling)' + info('aruco_detect/known_vertical = %s', known_vertical) + info('aruco_detect/flip_vertical = %s%s', flip_vertical, description) try: markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8) except rospy.ROSException: @@ -417,12 +420,15 @@ def check_aruco(): return if is_process_running('aruco_map', full=True): - known_tilt = rospy.get_param('aruco_map/known_tilt', '') - if known_tilt == 'map': - known_tilt += ' (marker\'s map is on the floor)' - elif known_tilt == 'map_flipped': - known_tilt += ' (marker\'s map is on the ceiling)' - info('aruco_map/known_tilt = %s', known_tilt) + known_vertical = rospy.get_param('aruco_map/known_vertical', '') + flip_vertical = rospy.get_param('aruco_map/flip_vertical', False) + description = '' + if known_vertical == 'map' and not flip_vertical: + description += ' (markers map is on the floor)' + elif known_vertical == 'map' and flip_vertical: + description += ' (markers map is on the ceiling)' + info('aruco_map/known_vertical = %s', known_vertical) + info('aruco_map/flip_vertical = %s%s', flip_vertical, description) try: visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8) @@ -462,7 +468,8 @@ def check_vpe(): except rospy.ROSException: if not is_process_running('vpe_publisher', full=True): info('no vision position estimate, vpe_publisher is not running') - elif rospy.get_param('aruco_map/known_tilt', '') == 'map_flipped': + elif rospy.get_param('aruco_map/known_vertical', '') == 'map' \ + and rospy.get_param('aruco_map/flip_vertical', False): failure('no vision position estimate, markers are on the ceiling') elif is_on_the_floor(): info('no vision position estimate, the drone is on the floor')