From 82d9840bc16dd380e2509e4fdac6d0be041f1796 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:36 +0900 Subject: [PATCH 1/7] refactor(behavior_path_planner): separate processing time function (#5256) Signed-off-by: Takamasa Horibe --- .../include/behavior_path_planner/planner_manager.hpp | 5 +++++ .../src/behavior_path_planner_node.cpp | 1 + planning/behavior_path_planner/src/planner_manager.cpp | 10 ++++++++-- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index ee5771c98217e..72c32039da627 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -242,6 +242,11 @@ class PlannerManager */ void print() const; + /** + * @brief publish processing time of each module. + */ + void publishProcessingTime() const; + /** * @brief visit each module and get debug information. */ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 6e37bd0ec4b2c..755817aa22ae7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -579,6 +579,7 @@ void BehaviorPathPlannerNode::run() lk_pd.unlock(); // release planner_data_ planner_manager_->print(); + planner_manager_->publishProcessingTime(); planner_manager_->publishMarker(); planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 5963ddb778874..24967608775f3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -886,13 +886,19 @@ void PlannerManager::print() const string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first << ":" << std::setw(4) << std::right << t.second << "ms]\n" << std::setw(21); - std::string name = std::string("processing_time/") + t.first; - debug_publisher_ptr_->publish(name, t.second); } RCLCPP_INFO_STREAM(logger_, string_stream.str()); } +void PlannerManager::publishProcessingTime() const +{ + for (const auto & t : processing_time_) { + std::string name = std::string("processing_time/") + t.first; + debug_publisher_ptr_->publish(name, t.second); + } +} + std::shared_ptr PlannerManager::getDebugMsg() { debug_msg_ptr_ = std::make_shared(); From 2aa9d1cd68b81df7b154195e7243f3e872256b8e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:54 +0900 Subject: [PATCH 2/7] chore(obstacle_velocity_limiter): publish processing time as float (#5257) Signed-off-by: Takamasa Horibe --- planning/obstacle_velocity_limiter/README.md | 2 +- .../obstacle_velocity_limiter_node.hpp | 4 ++-- .../src/obstacle_velocity_limiter_node.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index d670f07d26e9d..36aa35bb31156 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -161,7 +161,7 @@ For example a value of `1` means all trajectory points will be evaluated while a | ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | | `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | | `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `std_msgs/Int64` | Time taken to calculate the trajectory (in microseconds) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index a2ff72d0da562..cbdd390183d61 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -56,7 +56,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_runtime_; //!< @brief publisher for callback runtime rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index f4601979b9b8a..086d76e9ef0fe 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -68,7 +68,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); + pub_runtime_ = + create_publisher("~/output/runtime_microseconds", 1); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -223,7 +224,8 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); - pub_runtime_->publish(std_msgs::msg::Int64().set__data(runtime.count())); + pub_runtime_->publish(tier4_debug_msgs::msg::Float64Stamped().set__stamp(now()).set__data( + static_cast(runtime.count()))); if (pub_debug_markers_->get_subscription_count() > 0) { const auto safe_projected_linestrings = From 3a8ff49eb0da121c6c0a28546f0853d5853f57d4 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 11 Oct 2023 10:09:48 +0900 Subject: [PATCH 3/7] fix(map_projection_loader): fix sample config parameter in readme (#5262) fix readme Signed-off-by: Kento Yabuuchi --- map/map_projection_loader/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 0b0c41821d036..94b142da3dcf9 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -25,7 +25,7 @@ sample-map-rosbag ```yaml # map_projector_info.yaml -type: "local" +projector_type: local ``` ### Using MGRS From 671cc9658f8ccf52de03089f7b1ddc9e6c59a2a5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 11 Oct 2023 15:58:07 +0900 Subject: [PATCH 4/7] feat(intersection): find stop line from footprint intersection with areas (#5264) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 3 +- .../src/scene_merge_from_private_road.cpp | 6 +- .../src/util.cpp | 106 +++++++++++++----- .../src/util_type.hpp | 6 +- 4 files changed, 87 insertions(+), 34 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75bb4e861a60f..f5eda0d0fe4d7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -877,7 +877,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info); + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index bb54d829cc477..698242da67528 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -94,8 +94,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, planner_param_.consider_wrong_direction_vehicle); } - intersection_lanelets_.value().update(false, interpolated_path_info); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); + auto & intersection_lanelets = intersection_lanelets_.value(); + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(false, interpolated_path_info, local_footprint); + const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 2f3fcc6f6fde0..7862a533f34eb 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -220,6 +220,50 @@ static std::optional getStopLineIndexFromMap( return stop_idx_ip_opt.get(); } +static std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = lane_start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + for (size_t i = lane_start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & polygon : polygons) { + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>( + i, polygon); + } + } + } + return std::nullopt; +} + std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::CompoundPolygon3d & first_detection_area, @@ -236,6 +280,7 @@ std::optional generateIntersectionStopLines( const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); + /* // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); @@ -244,6 +289,16 @@ std::optional generateIntersectionStopLines( return std::nullopt; } const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); + */ + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_detection_area, interpolated_path_info, local_footprint); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); // (1) default stop line position on interpolated path bool default_stop_line_valid = true; @@ -254,8 +309,7 @@ std::optional generateIntersectionStopLines( stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - - base2front_idx_dist; + stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stop_line_valid = false; @@ -272,8 +326,6 @@ std::optional generateIntersectionStopLines( const auto closest_idx_ip = closest_idx_ip_opt.value(); // (3) occlusion peeking stop line position on interpolated path - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; { @@ -281,20 +333,13 @@ std::optional generateIntersectionStopLines( const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects(path_footprint0, area_2d)) { + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; - } - } + occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; } const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; @@ -310,8 +355,8 @@ std::optional generateIntersectionStopLines( const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); - int pass_judge_ip_int = static_cast(first_inside_detection_ip) - base2front_idx_dist - - std::ceil(braking_dist / ds); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -321,18 +366,18 @@ std::optional generateIntersectionStopLines( if (use_stuck_stopline) { // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. - const auto stuck_stop_line_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); + const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; } else { - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; } } else { - stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); + stuck_stop_line_ip_int = + std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); } - stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); if (stuck_stop_line_ip_int < 0) { stuck_stop_line_valid = false; } @@ -1243,21 +1288,22 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info) + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - { - auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_); - if (first && !first_conflicting_area_) { + if (!first_conflicting_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + if (first) { first_conflicting_area_ = first.value().second; } } - { - auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_); - if (first && !first_attention_area_) { + if (!first_attention_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(attention_area_, interpolated_path_info, footprint); + if (first) { first_attention_area_ = first.value().second; } } diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index aed81d771e480..581f22904fd2b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -15,6 +15,8 @@ #ifndef UTIL_TYPE_HPP_ #define UTIL_TYPE_HPP_ +#include + #include #include #include @@ -67,7 +69,9 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info); + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { return is_prioritized_ ? attention_non_preceding_ : attention_; From 9fe7626889668b80298c7aeeba3068b6b622fc73 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 11 Oct 2023 15:58:22 +0900 Subject: [PATCH 5/7] fix(intersection): collision check considering object width (#5265) Signed-off-by: Takayuki Murooka --- .../src/scene_intersection.cpp | 35 ++++++++++++++++--- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index f5eda0d0fe4d7..fb35834ab39a5 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -43,6 +44,32 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +namespace +{ +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -1313,14 +1340,14 @@ bool IntersectionModule::checkCollision( // collision point const auto first_itr = std::adjacent_find( predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (first_itr == predicted_path.path.cend()) continue; const auto last_itr = std::adjacent_find( predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (last_itr == predicted_path.path.crend()) continue; From dc8cc07bfebeed7aef328891a720bfc17899726d Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Wed, 11 Oct 2023 15:48:44 +0800 Subject: [PATCH 6/7] feat(kalman_filter): add gtest (#4799) * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/kalman_filter/CMakeLists.txt | 7 + common/kalman_filter/package.xml | 2 + .../kalman_filter/test/test_kalman_filter.cpp | 94 ++++++++++++++ .../test/test_time_delay_kalman_filter.cpp | 121 ++++++++++++++++++ 4 files changed, 224 insertions(+) create mode 100644 common/kalman_filter/test/test_kalman_filter.cpp create mode 100644 common/kalman_filter/test/test_time_delay_kalman_filter.cpp diff --git a/common/kalman_filter/CMakeLists.txt b/common/kalman_filter/CMakeLists.txt index dd59da727605d..6cbdf67a0affa 100644 --- a/common/kalman_filter/CMakeLists.txt +++ b/common/kalman_filter/CMakeLists.txt @@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED include/kalman_filter/time_delay_kalman_filter.hpp ) +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_kalman_filter ${test_files}) + + target_link_libraries(test_kalman_filter kalman_filter) +endif() + ament_auto_package() diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index ea061f3f23bb8..200440b5774c7 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -18,7 +18,9 @@ eigen3_cmake_module ament_cmake_cppcheck + ament_cmake_ros ament_lint_auto + autoware_lint_common ament_cmake diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/kalman_filter/test/test_kalman_filter.cpp new file mode 100644 index 0000000000000..4f4e9ce44669b --- /dev/null +++ b/common/kalman_filter/test/test_kalman_filter.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 The 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 "kalman_filter/kalman_filter.hpp" + +#include + +TEST(kalman_filter, kf) +{ + KalmanFilter kf_; + + Eigen::MatrixXd x_t(2, 1); + x_t << 1, 2; + + Eigen::MatrixXd P_t(2, 2); + P_t << 1, 0, 0, 1; + + Eigen::MatrixXd Q_t(2, 2); + Q_t << 0.01, 0, 0, 0.01; + + Eigen::MatrixXd R_t(2, 2); + R_t << 0.09, 0, 0, 0.09; + + Eigen::MatrixXd C_t(2, 2); + C_t << 1, 0, 0, 1; + + Eigen::MatrixXd A_t(2, 2); + A_t << 1, 0, 0, 1; + + Eigen::MatrixXd B_t(2, 2); + B_t << 1, 0, 0, 1; + + // Initialize the filter and check if initialization was successful + EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); + + // Perform prediction + Eigen::MatrixXd u_t(2, 1); + u_t << 0.1, 0.1; + EXPECT_TRUE(kf_.predict(u_t)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; + Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; + + Eigen::MatrixXd x_predict; + kf_.getX(x_predict); + Eigen::MatrixXd P_predict; + kf_.getP(P_predict); + + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + + // Perform update + Eigen::MatrixXd y_t(2, 1); + y_t << 1.05, 2.05; + EXPECT_TRUE(kf_.update(y_t)); + + // Check the updated state and covariance matrix + const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_t * x_predict_expected; + Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); + Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); + + Eigen::MatrixXd x_update; + kf_.getX(x_update); + Eigen::MatrixXd P_update; + kf_.getP(P_update); + + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp new file mode 100644 index 0000000000000..32fefd8ceff70 --- /dev/null +++ b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 The 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 "kalman_filter/time_delay_kalman_filter.hpp" + +#include + +TEST(time_delay_kalman_filter, td_kf) +{ + TimeDelayKalmanFilter td_kf_; + + Eigen::MatrixXd x_t(3, 1); + x_t << 1.0, 2.0, 3.0; + Eigen::MatrixXd P_t(3, 3); + P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; + const int max_delay_step = 5; + const int dim_x = x_t.rows(); + const int dim_x_ex = dim_x * max_delay_step; + // Initialize the filter + td_kf_.init(x_t, P_t, max_delay_step); + + // Check if initialization was successful + Eigen::MatrixXd x_init = td_kf_.getLatestX(); + Eigen::MatrixXd P_init = td_kf_.getLatestP(); + Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); + Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + for (int i = 0; i < max_delay_step; ++i) { + x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; + P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; + } + + EXPECT_EQ(x_init.rows(), 3); + EXPECT_EQ(x_init.cols(), 1); + EXPECT_EQ(P_init.rows(), 3); + EXPECT_EQ(P_init.cols(), 3); + EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); + EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); + EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); + EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); + EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); + EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); + + // Define prediction parameters + Eigen::MatrixXd A_t(3, 3); + A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; + Eigen::MatrixXd Q_t(3, 3); + Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; + Eigen::MatrixXd x_next(3, 1); + x_next << 2.0, 4.0, 6.0; + + // Perform prediction + EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); + + // Check the prediction state and covariance matrix + Eigen::MatrixXd x_predict = td_kf_.getLatestX(); + Eigen::MatrixXd P_predict = td_kf_.getLatestP(); + Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); + x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; + x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); + x_ex_t = x_tmp; + Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; + P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = + A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); + P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); + P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); + P_ex_t = P_tmp; + Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); + + // Define update parameters + Eigen::MatrixXd C_t(3, 3); + C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; + Eigen::MatrixXd R_t(3, 3); + R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; + Eigen::MatrixXd y_t(3, 1); + y_t << 1.05, 2.05, 3.05; + const int delay_step = 2; // Choose an appropriate delay step + const int dim_y = y_t.rows(); + + // Perform update + EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_update = td_kf_.getLatestX(); + Eigen::MatrixXd P_update = td_kf_.getLatestP(); + + Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); + const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; + x_ex_t = x_ex_t + K_t * (y_t - y_pred); + P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); + Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); + EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); +} From 84995ec3ca4e201ed1d800c0eeb967de25298f84 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 11 Oct 2023 18:37:33 +0900 Subject: [PATCH 7/7] feat(intersection): not detect stuck vehicle when turning left (#5268) Signed-off-by: Takayuki Murooka --- .../src/scene_intersection.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index fb35834ab39a5..26bc8210b3bad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1213,6 +1213,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( bool IntersectionModule::checkStuckVehicle( const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { + // NOTE: No need to stop for stuck vehicle when the ego will turn left. + if (turn_direction_ == std::string("left")) { + return false; + } + const auto & objects_ptr = planner_data->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path