diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index a66277c699a00..3f7a2683ea906 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -44,22 +44,27 @@ #include "ar_tag_based_localizer.hpp" +#include "localization_util/pose_array_interpolator.hpp" #include "localization_util/util_func.hpp" #include #include #include +#include #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include + ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : Node("ar_tag_based_localizer", options), cam_info_received_(false) { @@ -73,7 +78,7 @@ bool ArTagBasedLocalizer::setup() marker_size_ = static_cast(this->declare_parameter("marker_size")); target_tag_ids_ = this->declare_parameter>("target_tag_ids"); base_covariance_ = this->declare_parameter>("base_covariance"); - distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); + distance_threshold_ = this->declare_parameter("distance_threshold"); ekf_time_tolerance_ = this->declare_parameter("ekf_time_tolerance"); ekf_position_tolerance_ = this->declare_parameter("ekf_position_tolerance"); std::string detection_mode = this->declare_parameter("detection_mode"); @@ -103,7 +108,6 @@ bool ArTagBasedLocalizer::setup() */ tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); - tf_broadcaster_ = std::make_unique(this); /* Initialize image transport @@ -119,6 +123,7 @@ bool ArTagBasedLocalizer::setup() rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); + pose_array_pub_ = this->create_publisher("~/debug/detected_landmarks", qos_sub); image_sub_ = this->create_subscription( "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); @@ -160,71 +165,85 @@ void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { + // check subscribers if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); return; } + // check cam_info if (!cam_info_received_) { RCLCPP_DEBUG(this->get_logger(), "No cam_info has been received."); return; } - const builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; - cv_bridge::CvImagePtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; + // get self pose + const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; + Pose self_pose; + { + // get self-position on map + std::unique_lock self_pose_array_lock(self_pose_array_mtx_); + if (self_pose_msg_ptr_array_.size() <= 1) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!"); + return; + } + PoseArrayInterpolator interpolator( + this, sensor_stamp, self_pose_msg_ptr_array_, ekf_time_tolerance_, ekf_position_tolerance_); + if (!interpolator.is_success()) { + return; + } + pop_old_pose(self_pose_msg_ptr_array_, sensor_stamp); + self_pose = interpolator.get_current_pose().pose.pose; } - cv::Mat in_image = cv_ptr->image; - - // detection results will go into "markers" - std::vector markers; - - // ok, let's detect - detector_.detect(in_image, markers, cam_param_, marker_size_, false); - - // for each marker, draw info and its boundaries in the image - for (const aruco::Marker & marker : markers) { - const tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); + // detect + const std::vector landmarks = detect_landmarks(msg); + if (landmarks.empty()) { + return; + } - TransformStamped tf_cam_to_marker_stamped; - tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); - tf_cam_to_marker_stamped.header.stamp = curr_stamp; - tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id; - tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); - tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); + // for debug + if (pose_array_pub_->get_subscription_count() > 0) { + PoseArray pose_array_msg; + pose_array_msg.header.stamp = sensor_stamp; + pose_array_msg.header.frame_id = "map"; + for (const landmark_manager::Landmark & landmark : landmarks) { + const Pose detected_marker_on_map = + tier4_autoware_utils::transformPose(landmark.pose, self_pose); + pose_array_msg.poses.push_back(detected_marker_on_map); + } + pose_array_pub_->publish(pose_array_msg); + } - PoseStamped pose_cam_to_marker; - tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); - pose_cam_to_marker.header.stamp = curr_stamp; - pose_cam_to_marker.header.frame_id = msg->header.frame_id; - publish_pose_as_base_link(pose_cam_to_marker, std::to_string(marker.id)); + // calc new_self_pose + const Pose new_self_pose = calculate_new_self_pose(landmarks, self_pose); + const Pose diff_pose = tier4_autoware_utils::inverseTransformPose(new_self_pose, self_pose); + const double distance = + std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); - // drawing the detected markers - marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + // check distance + if (distance > ekf_position_tolerance_) { + RCLCPP_INFO_STREAM(this->get_logger(), "distance: " << distance); + return; } - // draw a 3d cube in each marker if there is 3d info - if (cam_param_.isValid()) { - for (aruco::Marker & marker : markers) { - aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); - } - } + // publish + PoseWithCovarianceStamped pose_with_covariance_stamped; + pose_with_covariance_stamped.header.stamp = sensor_stamp; + pose_with_covariance_stamped.header.frame_id = "map"; + pose_with_covariance_stamped.pose.pose = new_self_pose; - if (image_pub_.getNumSubscribers() > 0) { - // show input with augmented information - cv_bridge::CvImage out_msg; - out_msg.header.stamp = curr_stamp; - out_msg.encoding = sensor_msgs::image_encodings::RGB8; - out_msg.image = in_image; - image_pub_.publish(out_msg.toImageMsg()); + // ~5[m]: base_covariance + // 5~[m]: scaling base_covariance by std::pow(distance / 5, 3) + const double coeff = std::max(1.0, std::pow(distance / 5, 3)); + for (int i = 0; i < 36; i++) { + pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; } - const int detected_tags = static_cast(markers.size()); + pose_pub_->publish(pose_with_covariance_stamped); + + // publish diagnostics + const int detected_tags = static_cast(landmarks.size()); diagnostic_msgs::msg::DiagnosticStatus diag_status; @@ -252,150 +271,178 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) } // wait for one camera info, then shut down that subscriber -void ArTagBasedLocalizer::cam_info_callback(const CameraInfo & msg) +void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & msg) { if (cam_info_received_) { return; } cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - camera_matrix.at(0, 0) = msg.p[0]; - camera_matrix.at(0, 1) = msg.p[1]; - camera_matrix.at(0, 2) = msg.p[2]; - camera_matrix.at(0, 3) = msg.p[3]; - camera_matrix.at(1, 0) = msg.p[4]; - camera_matrix.at(1, 1) = msg.p[5]; - camera_matrix.at(1, 2) = msg.p[6]; - camera_matrix.at(1, 3) = msg.p[7]; - camera_matrix.at(2, 0) = msg.p[8]; - camera_matrix.at(2, 1) = msg.p[9]; - camera_matrix.at(2, 2) = msg.p[10]; - camera_matrix.at(2, 3) = msg.p[11]; + camera_matrix.at(0, 0) = msg->p[0]; + camera_matrix.at(0, 1) = msg->p[1]; + camera_matrix.at(0, 2) = msg->p[2]; + camera_matrix.at(0, 3) = msg->p[3]; + camera_matrix.at(1, 0) = msg->p[4]; + camera_matrix.at(1, 1) = msg->p[5]; + camera_matrix.at(1, 2) = msg->p[6]; + camera_matrix.at(1, 3) = msg->p[7]; + camera_matrix.at(2, 0) = msg->p[8]; + camera_matrix.at(2, 1) = msg->p[9]; + camera_matrix.at(2, 2) = msg->p[10]; + camera_matrix.at(2, 3) = msg->p[11]; cv::Mat distortion_coeff(4, 1, CV_64FC1); for (int i = 0; i < 4; ++i) { distortion_coeff.at(i, 0) = 0; } - const cv::Size size(static_cast(msg.width), static_cast(msg.height)); + const cv::Size size(static_cast(msg->width), static_cast(msg->height)); cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); cam_info_received_ = true; } -void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped & msg) +void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg) { - latest_ekf_pose_ = msg; + // lock mutex for initial pose + std::lock_guard self_pose_array_lock(self_pose_array_mtx_); + // if rosbag restart, clear buffer + if (!self_pose_msg_ptr_array_.empty()) { + const builtin_interfaces::msg::Time & t_front = self_pose_msg_ptr_array_.front()->header.stamp; + const builtin_interfaces::msg::Time & t_msg = msg->header.stamp; + if (t_front.sec > t_msg.sec || (t_front.sec == t_msg.sec && t_front.nanosec > t_msg.nanosec)) { + self_pose_msg_ptr_array_.clear(); + } + } + + if (msg->header.frame_id == "map") { + self_pose_msg_ptr_array_.push_back(msg); + } else { + TransformStamped transform_self_pose_frame_to_map; + try { + transform_self_pose_frame_to_map = tf_buffer_->lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + + // transform self_pose_frame to map_frame + auto self_pose_on_map_ptr = std::make_shared(); + self_pose_on_map_ptr->pose.pose = + tier4_autoware_utils::transformPose(msg->pose.pose, transform_self_pose_frame_to_map); + // self_pose_on_map_ptr->pose.covariance; // TODO(YamatoAndo) + self_pose_on_map_ptr->header.stamp = msg->header.stamp; + self_pose_msg_ptr_array_.push_back(self_pose_on_map_ptr); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + get_logger(), "cannot get map to %s transform. %s", msg->header.frame_id.c_str(), + ex.what()); + } + } } -void ArTagBasedLocalizer::publish_pose_as_base_link( - const PoseStamped & sensor_to_tag, const std::string & tag_id) +std::vector ArTagBasedLocalizer::detect_landmarks( + const Image::ConstSharedPtr & msg) { - // Check tag_id - if (std::find(target_tag_ids_.begin(), target_tag_ids_.end(), tag_id) == target_tag_ids_.end()) { - RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in target_tag_ids"); - return; - } - if (landmark_map_.count(tag_id) == 0) { - RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in landmark_map_"); - return; - } + const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; - // Range filter - const double distance_squared = sensor_to_tag.pose.position.x * sensor_to_tag.pose.position.x + - sensor_to_tag.pose.position.y * sensor_to_tag.pose.position.y + - sensor_to_tag.pose.position.z * sensor_to_tag.pose.position.z; - if (distance_threshold_squared_ < distance_squared) { - return; + // get image + cv::Mat in_image; + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); + cv_ptr->image.copyTo(in_image); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return std::vector{}; } - // Transform to base_link - PoseStamped base_link_to_tag; + // get transform from base_link to camera + TransformStamped transform_sensor_to_base_link; try { - const TransformStamped transform = - tf_buffer_->lookupTransform("base_link", sensor_to_tag.header.frame_id, tf2::TimePointZero); - tf2::doTransform(sensor_to_tag, base_link_to_tag, transform); - base_link_to_tag.header.frame_id = "base_link"; + transform_sensor_to_base_link = + tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); - return; + return std::vector{}; } - // (1) map_to_tag - const Pose & map_to_tag = landmark_map_.at(tag_id); - const Eigen::Affine3d map_to_tag_affine = pose_to_affine3d(map_to_tag); - - // (2) tag_to_base_link - const Eigen::Affine3d base_link_to_tag_affine = pose_to_affine3d(base_link_to_tag.pose); - const Eigen::Affine3d tag_to_base_link_affine = base_link_to_tag_affine.inverse(); - - // calculate map_to_base_link - const Eigen::Affine3d map_to_base_link_affine = map_to_tag_affine * tag_to_base_link_affine; - const Pose map_to_base_link = tf2::toMsg(map_to_base_link_affine); - - // If latest_ekf_pose_ is older than seconds compared to current frame, it - // will not be published. - const rclcpp::Duration diff_time = - rclcpp::Time(sensor_to_tag.header.stamp) - rclcpp::Time(latest_ekf_pose_.header.stamp); - if (diff_time.seconds() > ekf_time_tolerance_) { - RCLCPP_INFO( - this->get_logger(), - "latest_ekf_pose_ is older than %f seconds compared to current frame. " - "latest_ekf_pose_.header.stamp: %d.%d, sensor_to_tag.header.stamp: %d.%d", - ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec, - sensor_to_tag.header.stamp.sec, sensor_to_tag.header.stamp.nanosec); - return; - } + // detect + std::vector markers; + detector_.detect(in_image, markers, cam_param_, marker_size_, false); - // If curr_pose differs from latest_ekf_pose_ by more than , it will not - // be published. - const Pose curr_pose = map_to_base_link; - const Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; - const double diff_position = norm(curr_pose.position, latest_ekf_pose.position); - if (diff_position > ekf_position_tolerance_) { - RCLCPP_INFO( - this->get_logger(), - "curr_pose differs from latest_ekf_pose_ by more than %f m. " - "curr_pose: (%f, %f, %f), latest_ekf_pose: (%f, %f, %f)", - ekf_position_tolerance_, curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, - latest_ekf_pose.position.x, latest_ekf_pose.position.y, latest_ekf_pose.position.z); - return; - } + // parse + std::vector landmarks; + + for (aruco::Marker & marker : markers) { + // convert marker pose to tf + const cv::Quat q = cv::Quat::createFromRvec(marker.Rvec); + Pose pose; + pose.position.x = marker.Tvec.at(0, 0); + pose.position.y = marker.Tvec.at(1, 0); + pose.position.z = marker.Tvec.at(2, 0); + pose.orientation.x = q.x; + pose.orientation.y = q.y; + pose.orientation.z = q.z; + pose.orientation.w = q.w; + const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z); + if (distance <= distance_threshold_) { + tf2::doTransform(pose, pose, transform_sensor_to_base_link); + landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose}); + } - // Construct output message - PoseWithCovarianceStamped pose_with_covariance_stamped; - pose_with_covariance_stamped.header.stamp = sensor_to_tag.header.stamp; - pose_with_covariance_stamped.header.frame_id = "map"; - pose_with_covariance_stamped.pose.pose = curr_pose; + // for debug, drawing the detected markers + marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); + } - // ~5[m]: base_covariance - // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) - const double distance = std::sqrt(distance_squared); - const double scale = distance / 5; - const double coeff = std::max(1.0, std::pow(scale, 3)); - for (int i = 0; i < 36; i++) { - pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; + // for debug + if (image_pub_.getNumSubscribers() > 0) { + cv_bridge::CvImage out_msg; + out_msg.header.stamp = sensor_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = in_image; + image_pub_.publish(out_msg.toImageMsg()); } - pose_pub_->publish(pose_with_covariance_stamped); + return landmarks; } -tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker) +geometry_msgs::msg::Pose ArTagBasedLocalizer::calculate_new_self_pose( + const std::vector & detected_landmarks, const Pose & self_pose) { - cv::Mat rot(3, 3, CV_64FC1); - cv::Mat r_vec64; - marker.Rvec.convertTo(r_vec64, CV_64FC1); - cv::Rodrigues(r_vec64, rot); - cv::Mat tran64; - marker.Tvec.convertTo(tran64, CV_64FC1); + Pose min_new_self_pose; + double min_distance = std::numeric_limits::max(); + + for (const landmark_manager::Landmark & landmark : detected_landmarks) { + // Firstly, landmark pose is base_link + const Pose & detected_marker_on_base_link = landmark.pose; - tf2::Matrix3x3 tf_rot( - rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), - rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), - rot.at(2, 2)); + // convert base_link to map + const Pose detected_marker_on_map = + tier4_autoware_utils::transformPose(detected_marker_on_base_link, self_pose); - tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + // match to map + if (landmark_map_.count(landmark.id) == 0) { + continue; + } + const Pose mapped_marker_on_map = landmark_map_[landmark.id]; + + // check distance + const double curr_distance = tier4_autoware_utils::calcDistance3d( + mapped_marker_on_map.position, detected_marker_on_map.position); + if (curr_distance > min_distance) { + continue; + } + + const Eigen::Affine3d marker_pose = pose_to_affine3d(mapped_marker_on_map); + const Eigen::Affine3d marker_to_base_link = + pose_to_affine3d(detected_marker_on_base_link).inverse(); + const Eigen::Affine3d new_self_pose_eigen = marker_pose * marker_to_base_link; + + const Pose new_self_pose = matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); + + // update + min_distance = curr_distance; + min_new_self_pose = new_self_pose; + } - return tf2::Transform(tf_rot, tf_orig); + return min_new_self_pose; } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index 889e76eb78ad2..2866f4a8f3ee7 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -51,6 +51,7 @@ #include #include +#include #include #include @@ -59,6 +60,7 @@ #include #include +#include #include #include #include @@ -72,6 +74,7 @@ class ArTagBasedLocalizer : public rclcpp::Node using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PoseArray = geometry_msgs::msg::PoseArray; using TransformStamped = geometry_msgs::msg::TransformStamped; using MarkerArray = visualization_msgs::msg::MarkerArray; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; @@ -83,23 +86,23 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); - void cam_info_callback(const CameraInfo & msg); - void ekf_pose_callback(const PoseWithCovarianceStamped & msg); - void publish_pose_as_base_link(const PoseStamped & sensor_to_tag, const std::string & tag_id); - static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); + void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); + void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); + Pose calculate_new_self_pose( + const std::vector & detected_landmarks, const Pose & self_pose); // Parameters float marker_size_{}; std::vector target_tag_ids_; std::vector base_covariance_; - double distance_threshold_squared_{}; + double distance_threshold_{}; double ekf_time_tolerance_{}; double ekf_position_tolerance_{}; // tf std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - std::unique_ptr tf_broadcaster_; // image transport std::unique_ptr it_; @@ -112,6 +115,7 @@ class ArTagBasedLocalizer : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr pose_array_pub_; image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; @@ -120,7 +124,8 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; - PoseWithCovarianceStamped latest_ekf_pose_{}; + std::mutex self_pose_array_mtx_; + std::deque self_pose_msg_ptr_array_; std::map landmark_map_; };