diff --git a/.github/labeler.yaml b/.github/labeler.yaml index c3efa2a1a2b15..47f8737ebbf39 100644 --- a/.github/labeler.yaml +++ b/.github/labeler.yaml @@ -31,7 +31,7 @@ - planning/**/* "component:sensing": - sensing/**/* -"component:simulator": +"component:simulation": - simulator/**/* "component:system": - system/**/* diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 4d316b9fb2481..5b60a371abd93 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -12,8 +12,6 @@ - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md - source: .github/dependabot.yaml - source: .github/stale.yml - pre-commands: | - sd "staleLabel: stale" "staleLabel: status:stale" {source} - source: .github/workflows/cancel-previous-workflows.yaml - source: .github/workflows/github-release.yaml - source: .github/workflows/pre-commit.yaml diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 5493f1dd594ce..2896286970217 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -24,9 +24,11 @@ #include #include +#include #include #include #include +#include #include namespace autoware @@ -45,10 +47,31 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); + ~PredictedObjectsDisplay() + { + { + std::unique_lock lock(queue_mutex); + should_terminate = true; + } + condition.notify_all(); + for (std::thread & active_thread : threads) { + active_thread.join(); + } + threads.clear(); + } private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; + void queueJob(std::function job) + { + { + std::unique_lock lock(queue_mutex); + jobs.push(std::move(job)); + } + condition.notify_one(); + } + boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { const std::string uuid_str = uuid_to_string(uuid_msg); @@ -100,6 +123,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay PredictedObjects::ConstSharedPtr msg); void workerThread(); + void messageProcessorThreadJob(); + void update(float wall_dt, float ros_dt) override; std::unordered_map> id_map; @@ -108,6 +133,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay int32_t marker_id = 0; const int32_t PATH_ID_CONSTANT = 1e3; + // max_num_threads: number of threads created in the thread pool, hard-coded to be 1; + int max_num_threads; + + bool should_terminate{false}; + std::mutex queue_mutex; + std::vector threads; + std::queue> jobs; + PredictedObjects::ConstSharedPtr msg; bool consumed{false}; std::mutex mutex; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 24e67a6f44e95..2cc5397d18721 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -25,27 +25,44 @@ namespace object_detection { PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") { - std::thread worker(&PredictedObjectsDisplay::workerThread, this); - worker.detach(); + max_num_threads = 1; // hard code the number of threads to be created + + for (int ii = 0; ii < max_num_threads; ++ii) { + threads.emplace_back(std::thread(&PredictedObjectsDisplay::workerThread, this)); + } } void PredictedObjectsDisplay::workerThread() -{ +{ // A standard working thread that waiting for jobs while (true) { - std::unique_lock lock(mutex); - condition.wait(lock, [this] { return this->msg; }); + std::function job; + { + std::unique_lock lock(queue_mutex); + condition.wait(lock, [this] { return !jobs.empty() || should_terminate; }); + if (should_terminate) { + return; + } + job = jobs.front(); + jobs.pop(); + } + job(); + } +} - auto tmp_msg = this->msg; - this->msg.reset(); +void PredictedObjectsDisplay::messageProcessorThreadJob() +{ + // Receiving + std::unique_lock lock(mutex); + auto tmp_msg = this->msg; + this->msg.reset(); + lock.unlock(); - lock.unlock(); + auto tmp_markers = createMarkers(tmp_msg); - auto tmp_markers = createMarkers(tmp_msg); - lock.lock(); - markers = tmp_markers; + lock.lock(); + markers = tmp_markers; - consumed = true; - } + consumed = true; } std::vector PredictedObjectsDisplay::createMarkers( @@ -188,7 +205,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms std::unique_lock lock(mutex); this->msg = msg; - condition.notify_one(); + queueJob(std::bind(&PredictedObjectsDisplay::messageProcessorThreadJob, this)); } void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 5d773c5be32d9..f4f9f45935347 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -168,6 +168,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) } T dst; + dst.reserve(points.size()); for (size_t i = 0; i <= start_idx; ++i) { dst.push_back(points.at(i)); @@ -1251,7 +1252,7 @@ calcLongitudinalOffsetPose boost::optional calcLongitudinalOffsetPose( diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp new file mode 100644 index 0000000000000..549ca4c0ae5bb --- /dev/null +++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -0,0 +1,77 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + +#include + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.points.push_back(p); + } + + return traj; +} +} // namespace + +TEST(trajectory_benchmark, DISABLED_calcLateralOffset) +{ + std::random_device r; + std::default_random_engine e1(r()); + std::uniform_real_distribution uniform_dist(0.0, 1000.0); + + using motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(1000, 1.0, 0.0, 0.0, 0.1); + constexpr auto nb_iteration = 10000; + for (auto i = 0; i < nb_iteration; ++i) { + const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0); + calcLateralOffset(traj.points, point); + } +} diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index c77503106e485..dc55576640133 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -46,7 +47,9 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::PoseDeviation; +using tier4_autoware_utils::Segment2d; using TrajectoryPoints = std::vector; +typedef boost::geometry::index::rtree> SegmentRtree; struct Param { @@ -137,13 +140,15 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); + double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const; + + static SegmentRtree extractUncrossableBoundaries( + const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, + const double max_search_length, const std::vector & boundary_types_to_detect); + static bool willCrossBoundary( - const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints, - const std::vector & boundary_types_to_detects); - - static bool isCrossingRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints); + const SegmentRtree & uncrossable_segments); }; } // namespace lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 9d413b2bf0a70..5bd0fcace9909 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -30,7 +30,9 @@ #include #include +using motion_utils::calcArcLength; using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiPoint2d; using tier4_autoware_utils::Point2d; @@ -41,15 +43,6 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Point; -Point fromVector2dToMsg(const Eigen::Vector2d & point) -{ - Point msg{}; - msg.x = point.x(); - msg.y = point.y(); - msg.z = 0.0; - return msg; -} - double calcBrakingDistance( const double abs_velocity, const double max_deceleration, const double delay_time) { @@ -67,27 +60,6 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithBoundary( - const lanelet::BasicLineString2d & boundary, const std::vector & footprints) -{ - for (auto & footprint : footprints) { - for (size_t i = 0; i < footprint.size() - 1; ++i) { - auto footprint1 = footprint.at(i).to_3d(); - auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < boundary.size() - 1; ++i) { - auto boundary1 = boundary.at(i); - auto boundary2 = boundary.at(i + 1); - if (tier4_autoware_utils::intersect( - tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { - return true; - } - } - } - } - return false; -} - LinearRing2d createHullFromFootprints(const std::vector & footprints) { MultiPoint2d combined; @@ -172,8 +144,12 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_boundary = willCrossBoundary( - output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + const double max_search_length_for_boundaries = + calcMaxSearchLengthForBoundaries(*input.predicted_trajectory); + const auto uncrossable_boundaries = extractUncrossableBoundaries( + *input.lanelet_map, input.predicted_trajectory->points.front().pose.position, + max_search_length_for_boundaries, input.boundary_types_to_detect); + output.will_cross_boundary = willCrossBoundary(output.vehicle_footprints, uncrossable_boundaries); output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; @@ -334,38 +310,58 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossBoundary( - const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints, - const std::vector & boundary_types_to_detect) +double LaneDepartureChecker::calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const { - for (const auto & candidate_lanelet : candidate_lanelets) { - const std::string r_type = - candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if ( - std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != - boundary_types_to_detect.end()) { - if (isCrossingWithBoundary( - candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { - // std::cerr << "The crossed road_border's line string id: " - // << candidate_lanelet.rightBound().id() << std::endl; - return true; + const double max_ego_lon_length = std::max( + std::abs(vehicle_info_ptr_->max_longitudinal_offset_m), + std::abs(vehicle_info_ptr_->min_longitudinal_offset_m)); + const double max_ego_lat_length = std::max( + std::abs(vehicle_info_ptr_->max_lateral_offset_m), + std::abs(vehicle_info_ptr_->min_lateral_offset_m)); + const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length); + return calcArcLength(trajectory.points) + max_ego_search_length; +} + +SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries( + const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, + const double max_search_length, const std::vector & boundary_types_to_detect) +{ + const auto has_types = + [](const lanelet::ConstLineString3d & ls, const std::vector & types) { + constexpr auto no_type = ""; + const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); + return (type != no_type && std::find(types.begin(), types.end(), type) != types.end()); + }; + + SegmentRtree uncrossable_segments_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; + for (const auto & ls : lanelet_map.lineStringLayer) { + if (has_types(ls, boundary_types_to_detect)) { + line.clear(); + for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); + for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) { + const Segment2d segment = {line[segment_idx], line[segment_idx + 1]}; + if (boost::geometry::distance(segment, ego_p) < max_search_length) { + uncrossable_segments_in_range.insert(segment); + } } } - const std::string l_type = - candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if ( - std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != - boundary_types_to_detect.end()) { - if (isCrossingWithBoundary( - candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { - // std::cerr << "The crossed road_border's line string id: " - // << candidate_lanelet.leftBound().id() << std::endl; - return true; - } + } + return uncrossable_segments_in_range; +} + +bool LaneDepartureChecker::willCrossBoundary( + const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments) +{ + for (const auto & footprint : vehicle_footprints) { + std::vector intersection_result; + uncrossable_segments.query( + boost::geometry::index::intersects(footprint), std::back_inserter(intersection_result)); + if (!intersection_result.empty()) { + return true; } } return false; } - } // namespace lane_departure_checker diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index c108bec2671b3..cc43da7114630 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -20,6 +20,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "trajectory_follower_node/visibility_control.hpp" @@ -38,6 +39,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include #include #include @@ -52,6 +54,8 @@ namespace trajectory_follower_node { using autoware_adapi_v1_msgs::msg::OperationModeState; +using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -78,6 +82,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_operation_mode_; rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; + rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; rclcpp::Publisher::SharedPtr debug_marker_pub_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; @@ -114,6 +120,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node const trajectory_follower::LateralOutput & lat_out) const; std::unique_ptr logger_configure_; + + void publishProcessingTime( + const double t_ms, const rclcpp::Publisher::SharedPtr pub); + StopWatch stop_watch_; }; } // namespace trajectory_follower_node } // namespace autoware::motion::control diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index e35848e9495af..322aa9eef5a79 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -75,6 +75,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); + pub_processing_time_lat_ms_ = + create_publisher("~/lateral/debug/processing_time_ms", 1); + pub_processing_time_lon_ms_ = + create_publisher("~/longitudinal/debug/processing_time_ms", 1); debug_marker_pub_ = create_publisher("~/output/debug_marker", rclcpp::QoS{1}); @@ -205,8 +209,13 @@ void Controller::callbackTimerControl() } // 3. run controllers + stop_watch_.tic("lateral"); const auto lat_out = lateral_controller_->run(*input_data); + publishProcessingTime(stop_watch_.toc("lateral"), pub_processing_time_lat_ms_); + + stop_watch_.tic("longitudinal"); const auto lon_out = longitudinal_controller_->run(*input_data); + publishProcessingTime(stop_watch_.toc("longitudinal"), pub_processing_time_lon_ms_); // 4. sync with each other controllers longitudinal_controller_->sync(lat_out.sync_data); @@ -254,6 +263,15 @@ void Controller::publishDebugMarker( debug_marker_pub_->publish(debug_marker_array); } +void Controller::publishProcessingTime( + const double t_ms, const rclcpp::Publisher::SharedPtr pub) +{ + Float64Stamped msg{}; + msg.stamp = this->now(); + msg.data = t_ms; + pub->publish(msg); +} + } // namespace autoware::motion::control::trajectory_follower_node #include "rclcpp_components/register_node_macro.hpp" diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml index 12d84f374f7c7..6cf8d390f919b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml @@ -145,9 +145,13 @@ - - - + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml deleted file mode 100644 index d4f82e72a297b..0000000000000 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 59fd5174fa38c..23a1201a84958 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -33,4 +33,12 @@ + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 9a137032e1277..c4de9c04dcaf2 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -16,15 +16,16 @@ ar_tag_based_localizer automatic_pose_initializer - eagleye_geo_pose_converter eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer + geo_pose_projector gyro_odometer ndt_scan_matcher pointcloud_preprocessor pose_initializer + pose_instability_detector topic_tools yabloc_common yabloc_image_processing diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 59fe3f13f1231..b3e9e9148ee60 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -195,6 +195,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 11deaad5d06cc..1b4c3f38e038d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -50,6 +50,7 @@ + @@ -78,9 +79,7 @@ - - - + @@ -220,6 +219,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 1fcf29606891b..b69f1c0ee8b14 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -32,6 +32,7 @@ + @@ -73,6 +74,7 @@ + @@ -104,6 +106,7 @@ + @@ -133,6 +136,7 @@ + @@ -142,6 +146,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 822c251ddad33..ac521934265d7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -61,6 +61,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index cee6829ea3b4a..ed37f6270c913 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -11,6 +11,7 @@ + @@ -37,9 +38,7 @@ - - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 5b5646a061ac7..26e7af07646cd 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -6,10 +6,8 @@ - - - + @@ -43,14 +41,6 @@ - - - - - - - - - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 347606d91759f..6582dfd32d91d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -12,12 +12,14 @@ + + @@ -102,7 +104,7 @@ @@ -174,6 +176,9 @@ + + + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 336588891d9b2..7e6ba83747a8b 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -39,6 +39,7 @@ shape_estimation tensorrt_yolo topic_tools + tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 085dc92982663..ba2318c438674 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -5,6 +5,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -23,6 +129,16 @@ + + + + + + + + + + @@ -66,6 +182,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 577b0e00d133c..18de04fd9e317 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -127,8 +127,8 @@ - - + + @@ -180,7 +180,7 @@ - + diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 45d420bafff10..9944cbb84d97c 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -43,11 +43,11 @@ function(add_testcase filepath) endfunction() if(BUILD_TESTING) - add_ros_test( + add_launch_test( test/test_ekf_localizer_launch.py TIMEOUT "30" ) - add_ros_test( + add_launch_test( test/test_ekf_localizer_mahalanobis.py TIMEOUT "30" ) diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt new file mode 100644 index 0000000000000..6e2cdf32fb6c5 --- /dev/null +++ b/localization/geo_pose_projector/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(geo_pose_projector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(geo_pose_projector + src/geo_pose_projector_node.cpp + src/geo_pose_projector.cpp +) +ament_target_dependencies(geo_pose_projector) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/geo_pose_projector/README.md b/localization/geo_pose_projector/README.md new file mode 100644 index 0000000000000..a3363336da1a7 --- /dev/null +++ b/localization/geo_pose_projector/README.md @@ -0,0 +1,28 @@ +# geo_pose_projector + +## Overview + +This node is a simple node that subscribes to the geo-referenced pose topic and publishes the pose in the map frame. + +## Subscribed Topics + +| Name | Type | Description | +| ------------------------- | ---------------------------------------------------- | ------------------- | +| `input_geo_pose` | `geographic_msgs::msg::GeoPoseWithCovarianceStamped` | geo-referenced pose | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectedObjectInfo` | map projector info | + +## Published Topics + +| Name | Type | Description | +| ------------- | ----------------------------------------------- | ------------------------------------- | +| `output_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | pose in map frame | +| `/tf` | `tf2_msgs::msg::TFMessage` | tf from parent link to the child link | + +## Parameters + +{{ json_to_markdown("localization/geo_pose_projector/schema/geo_pose_projector.schema.json") }} + +## Limitations + +The covariance conversion may be incorrect depending on the projection type you are using. The covariance of input topic is expressed in (Latitude, Longitude, Altitude) as a diagonal matrix. +Currently, we assume that the x axis is the east direction and the y axis is the north direction. Thus, the conversion may be incorrect when this assumption breaks, especially when the covariance of latitude and longitude is different. diff --git a/localization/geo_pose_projector/config/geo_pose_projector.param.yaml b/localization/geo_pose_projector/config/geo_pose_projector.param.yaml new file mode 100644 index 0000000000000..704d9dce8a4ee --- /dev/null +++ b/localization/geo_pose_projector/config/geo_pose_projector.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + publish_tf: true + parent_frame: "map" + child_frame: "pose_estimator_base_link" diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml new file mode 100644 index 0000000000000..d840add1ed1c7 --- /dev/null +++ b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml new file mode 100644 index 0000000000000..d424ff14b9c99 --- /dev/null +++ b/localization/geo_pose_projector/package.xml @@ -0,0 +1,30 @@ + + + + geo_pose_projector + 0.1.0 + The geo_pose_projector package + Yamato Ando + Koji Minoda + Apache License 2.0 + Koji Minoda + + ament_cmake_auto + autoware_cmake + + component_interface_specs + component_interface_utils + geographic_msgs + geography_utils + geometry_msgs + rclcpp + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/geo_pose_projector/schema/geo_pose_projector.schema.json b/localization/geo_pose_projector/schema/geo_pose_projector.schema.json new file mode 100644 index 0000000000000..9daafb27f4320 --- /dev/null +++ b/localization/geo_pose_projector/schema/geo_pose_projector.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for geo_pose_projector", + "type": "object", + "definitions": { + "geo_pose_projector": { + "type": "object", + "properties": { + "publish_tf": { + "type": "boolean", + "description": "whether to publish tf", + "default": true + }, + "parent_frame": { + "type": "string", + "description": "parent frame for published tf", + "default": "map" + }, + "child_frame": { + "type": "string", + "description": "child frame for published tf", + "default": "pose_estimator_base_link" + } + }, + "required": ["publish_tf", "parent_frame", "child_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/geo_pose_projector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp new file mode 100644 index 0000000000000..d147888bb743b --- /dev/null +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "geo_pose_projector.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +GeoPoseProjector::GeoPoseProjector() +: Node("geo_pose_projector"), publish_tf_(declare_parameter("publish_tf")) +{ + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; }); + + // Subscribe to geo_pose topic + geo_pose_sub_ = create_subscription( + "input_geo_pose", 10, + [this](const GeoPoseWithCovariance::ConstSharedPtr msg) { on_geo_pose(msg); }); + + // Publish pose topic + pose_pub_ = create_publisher("output_pose", 10); + + // Publish tf + if (publish_tf_) { + tf_broadcaster_ = std::make_unique(this); + parent_frame_ = declare_parameter("parent_frame"); + child_frame_ = declare_parameter("child_frame"); + } +} + +void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg) +{ + if (!projector_info_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000 /* ms */, "map_projector_info is not received yet."); + return; + } + + // get position + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = msg->pose.pose.position.latitude; + gps_point.longitude = msg->pose.pose.position.longitude; + gps_point.altitude = msg->pose.pose.position.altitude; + geometry_msgs::msg::Point position = + geography_utils::project_forward(gps_point, projector_info_.value()); + position.z = geography_utils::convert_height( + position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, + projector_info_.value().vertical_datum); + + // Convert geo_pose to pose + PoseWithCovariance projected_pose; + projected_pose.header = msg->header; + projected_pose.pose.pose.position = position; + projected_pose.pose.pose.orientation = msg->pose.pose.orientation; + projected_pose.pose.covariance = msg->pose.covariance; + + // Covariance in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate. + // TODO(TIER IV): This swap may be invalid when using other projector type. + projected_pose.pose.covariance[0] = msg->pose.covariance[7]; + projected_pose.pose.covariance[7] = msg->pose.covariance[0]; + + pose_pub_->publish(projected_pose); + + // Publish tf + if (publish_tf_) { + tf2::Transform transform; + transform.setOrigin(tf2::Vector3( + projected_pose.pose.pose.position.x, projected_pose.pose.pose.position.y, + projected_pose.pose.pose.position.z)); + const auto localization_quat = tf2::Quaternion( + projected_pose.pose.pose.orientation.x, projected_pose.pose.pose.orientation.y, + projected_pose.pose.pose.orientation.z, projected_pose.pose.pose.orientation.w); + transform.setRotation(localization_quat); + + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header = msg->header; + transform_stamped.header.frame_id = parent_frame_; + transform_stamped.child_frame_id = child_frame_; + transform_stamped.transform = tf2::toMsg(transform); + tf_broadcaster_->sendTransform(transform_stamped); + } +} diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp new file mode 100644 index 0000000000000..b0611fec36984 --- /dev/null +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEO_POSE_PROJECTOR_HPP_ +#define GEO_POSE_PROJECTOR_HPP_ + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +class GeoPoseProjector : public rclcpp::Node +{ +private: + using GeoPoseWithCovariance = geographic_msgs::msg::GeoPoseWithCovarianceStamped; + using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped; + using MapProjectorInfo = map_interface::MapProjectorInfo; + +public: + GeoPoseProjector(); + +private: + void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg); + + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; + rclcpp::Subscription::SharedPtr geo_pose_sub_; + rclcpp::Publisher::SharedPtr pose_pub_; + + std::unique_ptr tf_broadcaster_; + + std::optional projector_info_ = std::nullopt; + + const bool publish_tf_; + + std::string parent_frame_; + std::string child_frame_; +}; + +#endif // GEO_POSE_PROJECTOR_HPP_ diff --git a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp b/localization/geo_pose_projector/src/geo_pose_projector_node.cpp new file mode 100644 index 0000000000000..97367d6b9f774 --- /dev/null +++ b/localization/geo_pose_projector/src/geo_pose_projector_node.cpp @@ -0,0 +1,29 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "geo_pose_projector.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 59a5e1121b22f..57589837dd529 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -32,4 +32,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index ec50a113af4d9..e9e390010f084 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -21,10 +21,7 @@ ## Parameters -| Parameter | Type | Description | -| --------------------- | ------ | -------------------------------- | -| `output_frame` | String | output's frame id | -| `message_timeout_sec` | Double | delay tolerance time for message | +{{ json_to_markdown("localization/gyro_odometer/schema/gyro_odometer.schema.json") }} ## Assumptions / Known limits diff --git a/localization/gyro_odometer/config/gyro_odometer.param.yaml b/localization/gyro_odometer/config/gyro_odometer.param.yaml new file mode 100644 index 0000000000000..420a9b0d80582 --- /dev/null +++ b/localization/gyro_odometer/config/gyro_odometer.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + output_frame: "base_link" + message_timeout_sec: 0.2 diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index aeb505270b2cc..21aff3e10d26c 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -9,8 +9,7 @@ - - + @@ -23,7 +22,6 @@ - - + diff --git a/localization/gyro_odometer/schema/gyro_odometer.schema.json b/localization/gyro_odometer/schema/gyro_odometer.schema.json new file mode 100644 index 0000000000000..189df2a2f63f3 --- /dev/null +++ b/localization/gyro_odometer/schema/gyro_odometer.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for gyro odometer", + "type": "object", + "definitions": { + "gyro_odometer": { + "type": "object", + "properties": { + "output_frame": { + "type": "string", + "description": "output's frame id", + "default": "base_link" + }, + "message_timeout_sec": { + "type": "number", + "description": "delay tolerance time for message", + "default": 0.2 + } + }, + "required": ["output_frame", "message_timeout_sec"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/gyro_odometer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index c14c48ffb2046..5de0ecd7cdc0a 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -102,8 +102,8 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) : Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame", "base_link")), - message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), vehicle_twist_arrived_(false), imu_arrived_(false) { diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index 35d8d78e7015c..e4c79b7941b12 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -23,11 +23,11 @@ This calculated ego pose is passed to the EKF, where it is fused with the twist ![node diagram](./doc_image/node_diagram.drawio.svg) -### `landmark_parser` +### `landmark_manager` The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. -The `landmark_parser` is a utility package to load landmarks from the map. +The `landmark_manager` is a utility package to load landmarks from the map. - Translation : The center of the four vertices of the landmark - Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. @@ -43,7 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_parser` can interpret. +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret. The four vertices of a landmark are defined counterclockwise. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index c0d1b00e35d3b..7e83220dadf2a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -13,12 +13,14 @@ ament_cmake autoware_cmake + ament_index_cpp aruco cv_bridge diagnostic_msgs image_transport - landmark_parser + landmark_manager lanelet2_extension + localization_util rclcpp tf2_eigen tf2_geometry_msgs 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 73e7b8b7e3587..a66277c699a00 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,6 +44,8 @@ #include "ar_tag_based_localizer.hpp" +#include "localization_util/util_func.hpp" + #include #include #include @@ -111,18 +113,19 @@ bool ArTagBasedLocalizer::setup() /* Subscribers */ - map_bin_sub_ = this->create_subscription( + map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); + rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); - image_sub_ = this->create_subscription( + image_sub_ = this->create_subscription( "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); - cam_info_sub_ = this->create_subscription( + cam_info_sub_ = this->create_subscription( "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); - ekf_pose_sub_ = this->create_subscription( + ekf_pose_sub_ = this->create_subscription( "~/input/ekf_pose", qos_sub, std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); @@ -132,29 +135,30 @@ bool ArTagBasedLocalizer::setup() rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); qos_marker.transient_local(); qos_marker.reliable(); - marker_pub_ = - this->create_publisher("~/debug/marker", qos_marker); + marker_pub_ = this->create_publisher("~/debug/marker", qos_marker); rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); image_pub_ = it_->advertise("~/debug/result", 1); - pose_pub_ = this->create_publisher( - "~/output/pose_with_covariance", qos_pub); - diag_pub_ = - this->create_publisher("/diagnostics", qos_pub); + pose_pub_ = + this->create_publisher("~/output/pose_with_covariance", qos_pub); + diag_pub_ = this->create_publisher("/diagnostics", qos_pub); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); return true; } -void ArTagBasedLocalizer::map_bin_callback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) +void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger()); - const visualization_msgs::msg::MarkerArray marker_msg = - convert_to_marker_array_msg(landmark_map_); + const std::vector landmarks = + landmark_manager::parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + for (const landmark_manager::Landmark & landmark : landmarks) { + landmark_map_[landmark.id] = landmark.pose; + } + + const MarkerArray marker_msg = landmark_manager::convert_landmarks_to_marker_array_msg(landmarks); marker_pub_->publish(marker_msg); } -void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); @@ -166,7 +170,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha return; } - builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; + 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); @@ -185,20 +189,20 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha // for each marker, draw info and its boundaries in the image for (const aruco::Marker & marker : markers) { - tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); + const tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); - geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped; + 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); - geometry_msgs::msg::PoseStamped pose_cam_to_marker; + 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 = std::to_string(marker.id); - publish_pose_as_base_link(pose_cam_to_marker, msg->header.frame_id); + 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)); // drawing the detected markers marker.draw(in_image, cv::Scalar(0, 0, 255), 2); @@ -240,7 +244,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha key_value.value = std::to_string(detected_tags); diag_status.values.push_back(key_value); - diagnostic_msgs::msg::DiagnosticArray diag_msg; + DiagnosticArray diag_msg; diag_msg.header.stamp = this->now(); diag_msg.status.push_back(diag_status); @@ -248,17 +252,13 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha } // wait for one camera info, then shut down that subscriber -void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & msg) +void ArTagBasedLocalizer::cam_info_callback(const CameraInfo & msg) { if (cam_info_received_) { return; } cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - cv::Mat distortion_coeff(4, 1, CV_64FC1); - cv::Size size(static_cast(msg.width), static_cast(msg.height)); - - camera_matrix.setTo(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]; @@ -272,111 +272,88 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & 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)); + cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); cam_info_received_ = true; } -void ArTagBasedLocalizer::ekf_pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped & msg) { latest_ekf_pose_ = msg; } void ArTagBasedLocalizer::publish_pose_as_base_link( - const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id) + const PoseStamped & sensor_to_tag, const std::string & tag_id) { - // Check if frame_id is in target_tag_ids - if ( - std::find(target_tag_ids_.begin(), target_tag_ids_.end(), msg.header.frame_id) == - target_tag_ids_.end()) { - RCLCPP_INFO_STREAM( - this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in target_tag_ids"); + // 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; } // Range filter - const double distance_squared = msg.pose.position.x * msg.pose.position.x + - msg.pose.position.y * msg.pose.position.y + - msg.pose.position.z * msg.pose.position.z; + 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; } - // Transform map to tag - if (landmark_map_.count(msg.header.frame_id) == 0) { - RCLCPP_INFO_STREAM( - this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in landmark_map_"); - return; - } - const geometry_msgs::msg::Pose & tag_pose = landmark_map_.at(msg.header.frame_id); - geometry_msgs::msg::TransformStamped map_to_tag_tf; - map_to_tag_tf.header.stamp = msg.header.stamp; - map_to_tag_tf.header.frame_id = "map"; - map_to_tag_tf.child_frame_id = msg.header.frame_id; - map_to_tag_tf.transform.translation.x = tag_pose.position.x; - map_to_tag_tf.transform.translation.y = tag_pose.position.y; - map_to_tag_tf.transform.translation.z = tag_pose.position.z; - map_to_tag_tf.transform.rotation = tag_pose.orientation; - - // Transform camera to base_link - geometry_msgs::msg::TransformStamped camera_to_base_link_tf; + // Transform to base_link + PoseStamped base_link_to_tag; try { - camera_to_base_link_tf = - tf_buffer_->lookupTransform(camera_frame_id, "base_link", tf2::TimePointZero); + 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"; } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); return; } - // Convert ROS Transforms to Eigen matrices for easy matrix multiplication and inversion - Eigen::Affine3d map_to_tag = tf2::transformToEigen(map_to_tag_tf.transform); - Eigen::Affine3d camera_to_base_link = tf2::transformToEigen(camera_to_base_link_tf.transform); - Eigen::Affine3d camera_to_tag = Eigen::Affine3d( - Eigen::Translation3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) * - Eigen::Quaterniond( - msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, - msg.pose.orientation.z)); - Eigen::Affine3d tag_to_camera = camera_to_tag.inverse(); + // (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); - // Final pose calculation - Eigen::Affine3d map_to_base_link = map_to_tag * tag_to_camera * camera_to_base_link; + // (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(); - // Construct output message - geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance_stamped; - pose_with_covariance_stamped.header.stamp = msg.header.stamp; - pose_with_covariance_stamped.header.frame_id = "map"; - pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); + // 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 tolerance{ - static_cast(ekf_time_tolerance_), - static_cast((ekf_time_tolerance_ - std::floor(ekf_time_tolerance_)) * 1e9)}; - if (rclcpp::Time(latest_ekf_pose_.header.stamp) + tolerance < rclcpp::Time(msg.header.stamp)) { + 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, msg.header.stamp: %d.%d", + "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, - msg.header.stamp.sec, msg.header.stamp.nanosec); + sensor_to_tag.header.stamp.sec, sensor_to_tag.header.stamp.nanosec); return; } // If curr_pose differs from latest_ekf_pose_ by more than , it will not // be published. - const geometry_msgs::msg::Pose curr_pose = pose_with_covariance_stamped.pose.pose; - const geometry_msgs::msg::Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; - const double diff_x = curr_pose.position.x - latest_ekf_pose.position.x; - const double diff_y = curr_pose.position.y - latest_ekf_pose.position.y; - const double diff_z = curr_pose.position.z - latest_ekf_pose.position.z; - const double diff_distance_squared = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; - const double threshold = ekf_position_tolerance_ * ekf_position_tolerance_; - if (threshold < diff_distance_squared) { + 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. " @@ -386,6 +363,12 @@ void ArTagBasedLocalizer::publish_pose_as_base_link( return; } + // 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; + // ~5[m]: base_covariance // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) const double distance = std::sqrt(distance_squared); 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 6bd66eead653f..889e76eb78ad2 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 @@ -45,7 +45,7 @@ #ifndef AR_TAG_BASED_LOCALIZER_HPP_ #define AR_TAG_BASED_LOCALIZER_HPP_ -#include "landmark_parser/landmark_parser_core.hpp" +#include "landmark_manager/landmark_manager.hpp" #include #include @@ -66,17 +66,26 @@ class ArTagBasedLocalizer : public rclcpp::Node { + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using TransformStamped = geometry_msgs::msg::TransformStamped; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + public: explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); bool setup(); private: - void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); - void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); - void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); - void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); - void publish_pose_as_base_link( - const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id); + 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); // Parameters @@ -96,23 +105,23 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr it_; // Subscribers - rclcpp::Subscription::SharedPtr map_bin_sub_; - rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Subscription::SharedPtr cam_info_sub_; - rclcpp::Subscription::SharedPtr ekf_pose_sub_; + rclcpp::Subscription::SharedPtr map_bin_sub_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers - rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr pose_pub_; - rclcpp::Publisher::SharedPtr diag_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Others aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; - geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{}; - std::map landmark_map_; + PoseWithCovarianceStamped latest_ekf_pose_{}; + std::map landmark_map_; }; #endif // AR_TAG_BASED_LOCALIZER_HPP_ diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 6b302ee6bc862..1b44327fd9e8b 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -14,6 +14,7 @@ #include "../src/ar_tag_based_localizer.hpp" +#include #include #include @@ -29,8 +30,8 @@ class TestArTagBasedLocalizer : public ::testing::Test void SetUp() override { const std::string yaml_path = - "../../install/ar_tag_based_localizer/share/ar_tag_based_localizer/config/" - "ar_tag_based_localizer.param.yaml"; + ament_index_cpp::get_package_share_directory("ar_tag_based_localizer") + + "/config/ar_tag_based_localizer.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { diff --git a/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt similarity index 79% rename from localization/landmark_based_localizer/landmark_parser/CMakeLists.txt rename to localization/landmark_based_localizer/landmark_manager/CMakeLists.txt index 41f7c00d61383..1b6126c892d2e 100644 --- a/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(landmark_parser) +project(landmark_manager) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,8 +12,8 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(landmark_parser - src/landmark_parser_core.cpp +ament_auto_add_library(landmark_manager + src/landmark_manager.cpp ) ament_auto_package( diff --git a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp similarity index 64% rename from localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp rename to localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index edf45c2a2997a..9a22ee13f60ae 100644 --- a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -12,23 +12,35 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ -#define LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #include #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include +#include #include -#include #include +#include -std::map parse_landmark( +namespace landmark_manager +{ + +struct Landmark +{ + std::string id; + geometry_msgs::msg::Pose pose; +}; + +std::vector parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype, const rclcpp::Logger & logger); -visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( - const std::map & landmarks); +visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( + const std::vector & landmarks); + +} // namespace landmark_manager -#endif // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#endif // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ diff --git a/localization/landmark_based_localizer/landmark_parser/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml similarity index 88% rename from localization/landmark_based_localizer/landmark_parser/package.xml rename to localization/landmark_based_localizer/landmark_manager/package.xml index e3daa93f81220..1a35ae6a338d1 100644 --- a/localization/landmark_based_localizer/landmark_parser/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -1,9 +1,9 @@ - landmark_parser + landmark_manager 0.0.0 - The landmark_parser package + The landmark_manager package Shintaro Sakoda Masahiro Sakamoto Yamato Ando @@ -19,6 +19,7 @@ geometry_msgs lanelet2_extension rclcpp + tf2_eigen ament_cmake diff --git a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp similarity index 92% rename from localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp rename to localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index c86b35b6115b4..6d1698daf5eae 100644 --- a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -12,16 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "landmark_parser/landmark_parser_core.hpp" +#include "landmark_manager/landmark_manager.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include +#include #include #include -std::map parse_landmark( +namespace landmark_manager +{ + +std::vector parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype, const rclcpp::Logger & logger) { @@ -32,7 +36,7 @@ std::map parse_landmark( lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - std::map landmark_map; + std::vector landmarks; for (const auto & poly : lanelet_map_ptr->polygonLayer) { const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; @@ -93,8 +97,8 @@ std::map parse_landmark( pose.orientation.z = q.z(); pose.orientation.w = q.w(); - // Add to map - landmark_map[marker_id] = pose; + // Add + landmarks.push_back(Landmark{marker_id, pose}); RCLCPP_INFO_STREAM(logger, "id: " << marker_id); RCLCPP_INFO_STREAM( logger, @@ -104,11 +108,11 @@ std::map parse_landmark( << pose.orientation.z << ", " << pose.orientation.w); } - return landmark_map; + return landmarks; } -visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( - const std::map & landmarks) +visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( + const std::vector & landmarks) { int32_t id = 0; visualization_msgs::msg::MarkerArray marker_array; @@ -152,3 +156,5 @@ visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( } return marker_array; } + +} // namespace landmark_manager diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 6fe61cd25bc6e..4beecc2625fe3 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -38,7 +38,7 @@ link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) if(BUILD_TESTING) - add_ros_test( + add_launch_test( test/test_ndt_scan_matcher_launch.py TIMEOUT "30" ) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index f0d90fdfa0e08..6f9420f5bc71a 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -204,9 +204,10 @@ class NDTScanMatcher : public rclcpp::Node std::mutex initial_pose_array_mtx_; // variables for regularization - const bool regularization_enabled_; + const bool regularization_enabled_; // whether to use longitudinal regularization std::deque - regularization_pose_msg_ptr_array_; + regularization_pose_msg_ptr_array_; // queue for storing regularization base poses + std::mutex regularization_mutex_; // mutex for regularization_pose_msg_ptr_array_ bool is_activated_; std::shared_ptr tf2_listener_module_; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a2e89540a172f..1317b8bacf47f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -219,10 +219,20 @@ NDTScanMatcher::NDTScanMatcher() sensor_points_sub_ = this->create_subscription( "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); - regularization_pose_sub_ = - this->create_subscription( - "regularization_pose_with_covariance", 100, - std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1)); + + // Only if regularization is enabled, subscribe to the regularization base pose + if (regularization_enabled_) { + // NOTE: The reason that the regularization subscriber does not belong to the + // main_callback_group is to ensure that the regularization callback is called even if + // sensor_callback takes long time to process. + // Both callback_initial_pose and callback_regularization_pose must not miss receiving data for + // proper interpolation. + regularization_pose_sub_ = + this->create_subscription( + "regularization_pose_with_covariance", 10, + std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1), + initial_pose_sub_opt); + } sensor_aligned_pose_pub_ = this->create_publisher("points_aligned", 10); @@ -397,7 +407,10 @@ void NDTScanMatcher::callback_initial_pose( void NDTScanMatcher::callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) { + // Get lock for regularization_pose_msg_ptr_array_ + std::lock_guard lock(regularization_mutex_); regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr); + // Release lock for regularization_pose_msg_ptr_array_ } void NDTScanMatcher::callback_sensor_points( @@ -459,7 +472,9 @@ void NDTScanMatcher::callback_sensor_points( initial_pose_array_lock.unlock(); // if regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_) add_regularization_pose(sensor_ros_time); + if (regularization_enabled_) { + add_regularization_pose(sensor_ros_time); + } if (ndt_ptr_->getInputTarget() == nullptr) { RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); @@ -816,21 +831,29 @@ std::array NDTScanMatcher::estimate_covariance( std::optional NDTScanMatcher::interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time) { - if (regularization_pose_msg_ptr_array_.empty()) { - return std::nullopt; - } + std::shared_ptr interpolator = nullptr; + { + // Get lock for regularization_pose_msg_ptr_array_ + std::lock_guard lock(regularization_mutex_); - // synchronization - PoseArrayInterpolator interpolator(this, sensor_ros_time, regularization_pose_msg_ptr_array_); + if (regularization_pose_msg_ptr_array_.empty()) { + return std::nullopt; + } + + interpolator = std::make_shared( + this, sensor_ros_time, regularization_pose_msg_ptr_array_); - pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); + // Remove old poses to make next interpolation more efficient + pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); + + // Release lock for regularization_pose_msg_ptr_array_ + } - // if the interpolate_pose fails, 0.0 is stored in the stamp - if (rclcpp::Time(interpolator.get_current_pose().header.stamp).seconds() == 0.0) { + if (!interpolator || !interpolator->is_success()) { return std::nullopt; } - return pose_to_matrix4f(interpolator.get_current_pose().pose.pose); + return pose_to_matrix4f(interpolator->get_current_pose().pose.pose); } void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt new file mode 100644 index 0000000000000..5270df636d791 --- /dev/null +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(pose_instability_detector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(pose_instability_detector + src/main.cpp + src/pose_instability_detector.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_pose_instability_detector + test/test.cpp + src/pose_instability_detector.cpp + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md new file mode 100644 index 0000000000000..89cf6ca3be684 --- /dev/null +++ b/localization/pose_instability_detector/README.md @@ -0,0 +1,37 @@ +# pose_instability_detector + +The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). + +This node triggers periodic timer callbacks to compare two poses: + +- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The latest pose from `/localization/kinematic_state`. + +The results of this comparison are then output to the `/diagnostics` topic. + +If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. + +The following diagram provides an overview of what the timeline of this process looks like: + +![timeline](./media/timeline.drawio.svg) + +## Parameters + +{{ json_to_markdown("localization/pose_instability_detector/schema/pose_instability_detector.schema.json") }} + +## Input + +| Name | Type | Description | +| ------------------ | ---------------------------------------------- | --------------------- | +| `~/input/odometry` | nav_msgs::msg::Odometry | Pose estimated by EKF | +| `~/input/twist` | geometry_msgs::msg::TwistWithCovarianceStamped | Twist | + +## Output + +| Name | Type | Description | +| ------------------- | ------------------------------------- | ----------- | +| `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml new file mode 100644 index 0000000000000..29a25849d6b1c --- /dev/null +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + interval_sec: 1.0 # [sec] + threshold_diff_position_x: 1.0 # [m] + threshold_diff_position_y: 1.0 # [m] + threshold_diff_position_z: 1.0 # [m] + threshold_diff_angle_x: 1.0 # [rad] + threshold_diff_angle_y: 1.0 # [rad] + threshold_diff_angle_z: 1.0 # [rad] diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml new file mode 100644 index 0000000000000..4a390ee2854d7 --- /dev/null +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png new file mode 100644 index 0000000000000..b3ad402e48ba7 Binary files /dev/null and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/media/timeline.drawio.svg b/localization/pose_instability_detector/media/timeline.drawio.svg new file mode 100644 index 0000000000000..8fcdef3401025 --- /dev/null +++ b/localization/pose_instability_detector/media/timeline.drawio.svg @@ -0,0 +1,157 @@ + + + + + + + + Time + + + + + + timer_callback + + + + + + timer_callback + + + + + + prev_ekf + + + + + + latest_ekf + + + + + + ekf(ignored) + + + + + 1 sec + + + + + + ekf(ignored) + + + + + + ekf(ignored) + + + + ... + + + + About ekf + + + + + + Time + + + + + + timer_callback + + + + + + timer_callback + + + + + + prev_ekf + + + + + + latest_ekf + + + + + + twist + + + + + 1 sec + + + + + + twist + + + + + + twist + + + + ... + + + + About twist + + + + + + twist + (ignored) + + + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml new file mode 100644 index 0000000000000..7774407a7990d --- /dev/null +++ b/localization/pose_instability_detector/package.xml @@ -0,0 +1,35 @@ + + + + pose_instability_detector + 0.1.0 + The pose_instability_detector package + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto + Taiki Yamada + Ryu Yamamoto + Shintaro Sakoda + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + diagnostic_msgs + geometry_msgs + nav_msgs + rclcpp + tf2 + tf2_geometry_msgs + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json new file mode 100644 index 0000000000000..b45b43102444d --- /dev/null +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -0,0 +1,75 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Instability Detector Nodes", + "type": "object", + "definitions": { + "pose_instability_detector_node": { + "type": "object", + "properties": { + "interval_sec": { + "type": "number", + "default": 1.0, + "exclusiveMinimum": 0, + "description": "The interval of timer_callback in seconds." + }, + "threshold_diff_position_x": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position x (m)." + }, + "threshold_diff_position_y": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position y (m)." + }, + "threshold_diff_position_z": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position z (m)." + }, + "threshold_diff_angle_x": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle x (rad)." + }, + "threshold_diff_angle_y": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle y (rad)." + }, + "threshold_diff_angle_z": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle z (rad)." + } + }, + "required": [ + "interval_sec", + "threshold_diff_position_x", + "threshold_diff_position_y", + "threshold_diff_position_z", + "threshold_diff_angle_x", + "threshold_diff_angle_y", + "threshold_diff_angle_z" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_instability_detector_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp new file mode 100644 index 0000000000000..34e679e86730f --- /dev/null +++ b/localization/pose_instability_detector/src/main.cpp @@ -0,0 +1,26 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_instability_detector.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto pose_instability_detector = std::make_shared(); + rclcpp::spin(pose_instability_detector); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp new file mode 100644 index 0000000000000..afb7d6e007db2 --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -0,0 +1,176 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_instability_detector.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +#include + +PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) +: Node("pose_instability_detector", options), + threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), + threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), + threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), + threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), + threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), + threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) +{ + odometry_sub_ = this->create_subscription( + "~/input/odometry", 10, + std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); + + twist_sub_ = this->create_subscription( + "~/input/twist", 10, + std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); + + const double interval_sec = this->declare_parameter("interval_sec"); + timer_ = rclcpp::create_timer( + this, this->get_clock(), std::chrono::duration(interval_sec), + std::bind(&PoseInstabilityDetector::callback_timer, this)); + + diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); + diagnostics_pub_ = this->create_publisher("/diagnostics", 10); +} + +void PoseInstabilityDetector::callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr) +{ + latest_odometry_ = *odometry_msg_ptr; +} + +void PoseInstabilityDetector::callback_twist( + TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + twist_buffer_.push_back(*twist_msg_ptr); +} + +void PoseInstabilityDetector::callback_timer() +{ + if (latest_odometry_ == std::nullopt) { + return; + } + if (prev_odometry_ == std::nullopt) { + prev_odometry_ = latest_odometry_; + return; + } + + auto quat_to_rpy = [](const Quaternion & quat) { + tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3 mat(tf2_quat); + double roll{}; + double pitch{}; + double yaw{}; + mat.getRPY(roll, pitch, yaw); + return std::make_tuple(roll, pitch, yaw); + }; + + Pose pose = prev_odometry_->pose.pose; + rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); + for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { + const Twist twist = twist_with_cov.twist.twist; + + const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); + if (curr_time > latest_odometry_->header.stamp) { + break; + } + + const rclcpp::Duration time_diff = curr_time - prev_time; + const double time_diff_sec = time_diff.seconds(); + if (time_diff_sec < 0.0) { + continue; + } + + // quat to rpy + auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); + + // rpy update + ang_x += twist.angular.x * time_diff_sec; + ang_y += twist.angular.y * time_diff_sec; + ang_z += twist.angular.z * time_diff_sec; + tf2::Quaternion quat; + quat.setRPY(ang_x, ang_y, ang_z); + + // Convert twist to world frame + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(quat, linear_velocity); + + // update + pose.position.x += linear_velocity.x() * time_diff_sec; + pose.position.y += linear_velocity.y() * time_diff_sec; + pose.position.z += linear_velocity.z() * time_diff_sec; + pose.orientation.x = quat.x(); + pose.orientation.y = quat.y(); + pose.orientation.z = quat.z(); + pose.orientation.w = quat.w(); + prev_time = curr_time; + } + + // compare pose and latest_odometry_ + const Pose latest_ekf_pose = latest_odometry_->pose.pose; + const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_odom.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; + + const rclcpp::Time stamp = latest_odometry_->header.stamp; + + // publish diff_pose for debug + PoseStamped diff_pose; + diff_pose.header = latest_odometry_->header; + diff_pose.pose = ekf_to_odom; + diff_pose_pub_->publish(diff_pose); + + const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, + threshold_diff_position_z_, threshold_diff_angle_x_, + threshold_diff_angle_y_, threshold_diff_angle_z_}; + + const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", + "diff_angle_x", "diff_angle_y", "diff_angle_z"}; + + // publish diagnostics + DiagnosticStatus status; + status.name = "localization: pose_instability_detector"; + status.hardware_id = this->get_name(); + bool all_ok = true; + + for (size_t i = 0; i < values.size(); ++i) { + const bool ok = (std::abs(values[i]) < thresholds[i]); + all_ok &= ok; + diagnostic_msgs::msg::KeyValue kv; + kv.key = labels[i] + ":threshold"; + kv.value = std::to_string(thresholds[i]); + status.values.push_back(kv); + kv.key = labels[i] + ":value"; + kv.value = std::to_string(values[i]); + status.values.push_back(kv); + kv.key = labels[i] + ":status"; + kv.value = (ok ? "OK" : "WARN"); + status.values.push_back(kv); + } + status.level = (all_ok ? DiagnosticStatus::OK : DiagnosticStatus::WARN); + status.message = (all_ok ? "OK" : "WARN"); + + DiagnosticArray diagnostics; + diagnostics.header.stamp = stamp; + diagnostics.status.emplace_back(status); + diagnostics_pub_->publish(diagnostics); + + // prepare for next loop + prev_odometry_ = latest_odometry_; + twist_buffer_.clear(); +} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/src/pose_instability_detector.hpp new file mode 100644 index 0000000000000..761a10b7a6bf7 --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.hpp @@ -0,0 +1,70 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_INSTABILITY_DETECTOR_HPP_ +#define POSE_INSTABILITY_DETECTOR_HPP_ + +#include + +#include +#include +#include +#include + +#include + +class PoseInstabilityDetector : public rclcpp::Node +{ + using Quaternion = geometry_msgs::msg::Quaternion; + using Twist = geometry_msgs::msg::Twist; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Odometry = nav_msgs::msg::Odometry; + using KeyValue = diagnostic_msgs::msg::KeyValue; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); + void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_timer(); + + // subscribers and timer + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // publisher + rclcpp::Publisher::SharedPtr diff_pose_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + // parameters + const double threshold_diff_position_x_; + const double threshold_diff_position_y_; + const double threshold_diff_position_z_; + const double threshold_diff_angle_x_; + const double threshold_diff_angle_y_; + const double threshold_diff_angle_z_; + + // variables + std::optional latest_odometry_ = std::nullopt; + std::optional prev_odometry_ = std::nullopt; + std::vector twist_buffer_; +}; + +#endif // POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp new file mode 100644 index 0000000000000..5ea03859d7731 --- /dev/null +++ b/localization/pose_instability_detector/test/test.cpp @@ -0,0 +1,169 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pose_instability_detector.hpp" +#include "test_message_helper_node.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +class TestPoseInstabilityDetector : public ::testing::Test +{ + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Odometry = nav_msgs::msg::Odometry; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +protected: + void SetUp() override + { + const std::string yaml_path = + ament_index_cpp::get_package_share_directory("pose_instability_detector") + + "/config/pose_instability_detector.param.yaml"; + + rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); + if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { + std::cerr << "Failed to parse yaml file : " << yaml_path << std::endl; + std::exit(1); + } + + const rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(params_st, ""); + rclcpp::NodeOptions node_options; + for (const auto & param_pair : param_map) { + for (const auto & param : param_pair.second) { + node_options.parameter_overrides().push_back(param); + } + } + + subject_ = std::make_shared(node_options); + executor_.add_node(subject_); + + helper_ = std::make_shared(); + executor_.add_node(helper_); + + rcl_yaml_node_struct_fini(params_st); + } + + void TearDown() override + { + executor_.remove_node(subject_); + executor_.remove_node(helper_); + } + + rclcpp::executors::SingleThreadedExecutor executor_; + std::shared_ptr subject_; + std::shared_ptr helper_; +}; + +TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // NOLINT +{ + // send the first odometry message (start x = 10) + builtin_interfaces::msg::Time timestamp{}; + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // send the twist message2 (move 1m in x direction) + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // send the second odometry message (finish x = 12) + timestamp.sec = 2; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // check result + const diagnostic_msgs::msg::DiagnosticStatus & diagnostic_status = + helper_->received_diagnostic_array.status[0]; + EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOLINT +{ + // send the first odometry message (start x = 10) + builtin_interfaces::msg::Time timestamp{}; + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the twist message2 (move 0.1m in x direction) + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the second odometry message (finish x = 12) + timestamp.sec = 2; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // check result + const diagnostic_msgs::msg::DiagnosticStatus & diagnostic_status = + helper_->received_diagnostic_array.status[0]; + EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/pose_instability_detector/test/test_message_helper_node.hpp b/localization/pose_instability_detector/test/test_message_helper_node.hpp new file mode 100644 index 0000000000000..51444f9736b02 --- /dev/null +++ b/localization/pose_instability_detector/test/test_message_helper_node.hpp @@ -0,0 +1,80 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_MESSAGE_HELPER_NODE_HPP_ +#define TEST_MESSAGE_HELPER_NODE_HPP_ + +#include + +#include +#include +#include + +class TestMessageHelperNode : public rclcpp::Node +{ + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Odometry = nav_msgs::msg::Odometry; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + TestMessageHelperNode() : Node("test_message_helper_node") + { + odometry_publisher_ = + this->create_publisher("/pose_instability_detector/input/odometry", 10); + twist_publisher_ = this->create_publisher( + "/pose_instability_detector/input/twist", 10); + diagnostic_subscriber_ = this->create_subscription( + "/diagnostics", 10, + std::bind(&TestMessageHelperNode::callback_diagnostics, this, std::placeholders::_1)); + } + + void send_odometry_message( + const builtin_interfaces::msg::Time timestamp, const double x, const double y, const double z) + { + Odometry message{}; + message.header.stamp = timestamp; + message.pose.pose.position.x = x; + message.pose.pose.position.y = y; + message.pose.pose.position.z = z; + odometry_publisher_->publish(message); + } + + void send_twist_message( + const builtin_interfaces::msg::Time timestamp, const double x, const double y, const double z) + { + TwistWithCovarianceStamped message{}; + message.header.stamp = timestamp; + message.twist.twist.linear.x = x; + message.twist.twist.linear.y = y; + message.twist.twist.linear.z = z; + twist_publisher_->publish(message); + } + + void callback_diagnostics(const DiagnosticArray::ConstSharedPtr msg) + { + received_diagnostic_array = *msg; + received_diagnostic_array_flag = true; + } + + DiagnosticArray received_diagnostic_array; + bool received_diagnostic_array_flag = false; + +private: + rclcpp::Publisher::SharedPtr odometry_publisher_; + rclcpp::Publisher::SharedPtr twist_publisher_; + rclcpp::Subscription::SharedPtr diagnostic_subscriber_; +}; + +#endif // TEST_MESSAGE_HELPER_NODE_HPP_ diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index b94eaac7ee34d..115f7e5b17464 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -46,7 +46,7 @@ rclcpp_components_register_node(lanelet2_map_visualization_node ) if(BUILD_TESTING) - add_ros_test( + add_launch_test( test/lanelet2_map_loader_launch.test.py TIMEOUT "30" ) diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp index 358a5053b5d4a..eb5e73312a890 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -83,10 +83,18 @@ void ByteTrackNode::on_rect( bytetrack::ObjectArray objects = bytetrack_->update_tracker(object_array); for (const auto & tracked_object : objects) { tier4_perception_msgs::msg::DetectedObjectWithFeature object; - object.feature.roi.x_offset = tracked_object.x_offset; - object.feature.roi.y_offset = tracked_object.y_offset; - object.feature.roi.width = tracked_object.width; - object.feature.roi.height = tracked_object.height; + // fit xy offset to 0 if roi is outside of image + const int outside_x = std::max(-tracked_object.x_offset, 0); + const int outside_y = std::max(-tracked_object.y_offset, 0); + const int32_t output_x = std::max(tracked_object.x_offset, 0); + const int32_t output_y = std::max(tracked_object.y_offset, 0); + const int32_t output_width = tracked_object.width - outside_x; + const int32_t output_height = tracked_object.height - outside_y; + // convert int32 to uint32 + object.feature.roi.x_offset = static_cast(output_x); + object.feature.roi.y_offset = static_cast(output_y); + object.feature.roi.width = static_cast(output_width); + object.feature.roi.height = static_cast(output_height); object.object.existence_probability = tracked_object.score; object.object.classification.emplace_back( autoware_auto_perception_msgs::build