From f09e5751d1d74c5d47786129bb45f1021fb8e5e2 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 13 Jun 2023 12:40:52 +0900 Subject: [PATCH 01/15] fix(mission_planner): keep uuid when rerouting with modified goal (#3956) Signed-off-by: kosuke55 --- .../mission_planner/src/mission_planner/mission_planner.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 654f0129710da..c7073a38de075 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -373,8 +373,10 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt change_state(RouteState::Message::CHANGING); const std::vector empty_waypoints; - const auto new_route = + auto new_route = create_route(msg->header, empty_waypoints, msg->pose, normal_route_->allow_modification); + // create_route generate new uuid, so set the original uuid again to keep that. + new_route.uuid = msg->uuid; if (new_route.segments.empty()) { change_route(*normal_route_); change_state(RouteState::Message::SET); From fc8766eceaa823136fbbc0a2785b062bcac18438 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 13 Jun 2023 13:59:43 +0900 Subject: [PATCH 02/15] fix(bebavior_path_planner): fix turn signal direction in start_planner (#3958) Signed-off-by: tomoya.kimura --- .../src/scene_module/start_planner/start_planner_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index d478fca57f4b4..3564c7a63f4b7 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -806,10 +806,10 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose); if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else if ( distance_from_end < 0.0 && lateral_offset < -parameters_->th_blinker_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; } From 463d4e2e3f5531181ebb04483558227e9790ff04 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 13 Jun 2023 14:22:49 +0900 Subject: [PATCH 03/15] fix(behavior_path_planner): unable to fix bound (#3942) Signed-off-by: Fumiya Watanabe --- planning/behavior_path_planner/src/utils/utils.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index bfb795f42035d..54c47bcdbd22f 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1510,8 +1510,11 @@ void generateDrivableArea( } // make bound longitudinally monotonic - makeBoundLongitudinallyMonotonic(path, true); // for left bound - makeBoundLongitudinallyMonotonic(path, false); // for right bound + // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic + if (enable_expanding_polygon) { + makeBoundLongitudinallyMonotonic(path, true); // for left bound + makeBoundLongitudinallyMonotonic(path, false); // for right bound + } } // calculate bounds from drivable lanes and hatched road markings From d3bb389b164a11374f11aebcd7066069311add73 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 13 Jun 2023 14:39:05 +0900 Subject: [PATCH 04/15] feat(behavior_path_planner): enable lane change to activate turn signal when stopping (#3930) --- .../config/lane_change/lane_change.param.yaml | 1 + .../scene_module/lane_change/base_class.hpp | 6 ++ .../scene_module/lane_change/interface.hpp | 3 + .../lane_change/lane_change_module_data.hpp | 3 + .../src/behavior_path_planner_node.cpp | 4 + .../scene_module/lane_change/interface.cpp | 81 +++++++++++++++++++ 6 files changed, 98 insertions(+) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index d00a44c244815..2e55ab47ba75b 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -6,6 +6,7 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] + min_length_for_turn_signal_activation: 10.0 #[m] lane_changing_lateral_jerk: 0.5 # [m/s3] lateral_acc_switching_velocity: 4.0 #[m/s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 67aa9b3bfb5ea..61584ed06cd8b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -137,6 +137,8 @@ class LaneChangeBase const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } + LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } + bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; } bool isAbortEnabled() const { return lane_change_parameters_->enable_abort_lane_change; } @@ -165,6 +167,10 @@ class LaneChangeBase std::string getModuleTypeStr() const { return std::string{magic_enum::enum_name(type_)}; } + LaneChangeModuleType getModuleType() const { return type_; } + + TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; } + Direction getDirection() const { if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 70e4241458d53..fa7011bcdce27 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -93,6 +93,9 @@ class LaneChangeInterface : public SceneModuleInterface MarkerArray getModuleVirtualWall() override; + TurnSignalInfo getCurrentTurnSignalInfo( + const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info); + protected: std::shared_ptr parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index faa231d2930cd..d1c4482f0aff9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -35,6 +35,9 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // turn signal + double min_length_for_turn_signal_activation{10.0}; + // acceleration data double min_longitudinal_acc{-1.0}; double max_longitudinal_acc{1.0}; 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 539cb8cd52399..102ddac21edf8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -778,6 +778,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_acc_sampling_num = declare_parameter(parameter("lateral_acceleration_sampling_num")); + // min length + p.min_length_for_turn_signal_activation = + declare_parameter(parameter("min_length_for_turn_signal_activation")); + // acceleration p.min_longitudinal_acc = declare_parameter(parameter("min_longitudinal_acc")); p.max_longitudinal_acc = declare_parameter(parameter("max_longitudinal_acc")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index bcf4cf77543b4..536dee15252de 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -249,6 +249,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); + // change turn signal when the vehicle reaches at the end of the path for waiting lane change + out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); + if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); return out; @@ -407,6 +410,84 @@ void LaneChangeInterface::acceptVisitor(const std::shared_ptrgetLaneChangeStatus().lane_change_lanes; + const auto & is_valid = module_type_->getLaneChangeStatus().is_valid_path; + const auto & lane_change_path = module_type_->getLaneChangeStatus().lane_change_path; + const auto & lane_change_param = module_type_->getLaneChangeParam(); + + if ( + module_type_->getModuleType() != LaneChangeModuleType::NORMAL || target_lanes.empty() || + !is_valid) { + return original_turn_signal_info; + } + + // check direction + TurnSignalInfo current_turn_signal_info; + const auto & current_pose = module_type_->getEgoPose(); + const auto direction = module_type_->getDirection(); + if (direction == Direction::LEFT) { + current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else if (direction == Direction::RIGHT) { + current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + + if (path.points.empty()) { + current_turn_signal_info.desired_start_point = current_pose; + current_turn_signal_info.required_start_point = current_pose; + current_turn_signal_info.desired_end_point = lane_change_path.lane_changing_end; + current_turn_signal_info.required_end_point = lane_change_path.lane_changing_end; + return current_turn_signal_info; + } + + const auto & min_length_for_turn_signal_activation = + lane_change_param.min_length_for_turn_signal_activation; + const auto & route_handler = module_type_->getRouteHandler(); + const auto & common_parameter = module_type_->getCommonParam(); + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(target_lanes.back()); + const double next_lane_change_buffer = + utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; + const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; + + const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation; + const double path_length = motion_utils::calcArcLength(path.points); + const auto & front_point = path.points.front().point.pose.position; + const size_t & current_nearest_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double length_front_to_ego = motion_utils::calcSignedArcLength( + path.points, front_point, static_cast(0), current_pose.position, + current_nearest_seg_idx); + const auto start_pose = + motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); + if (path_length - length_front_to_ego < buffer && start_pose) { + // modify turn signal + current_turn_signal_info.desired_start_point = *start_pose; + current_turn_signal_info.desired_end_point = lane_change_path.lane_changing_end; + current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point; + current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point; + + const auto & original_command = original_turn_signal_info.turn_signal.command; + if ( + original_command == TurnIndicatorsCommand::DISABLE || + original_command == TurnIndicatorsCommand::NO_COMMAND) { + return current_turn_signal_info; + } + + // check the priority of turn signals + return module_type_->getTurnSignalDecider().use_prior_turn_signal( + path, current_pose, current_nearest_seg_idx, original_turn_signal_info, + current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); + } + + // not in the vicinity of the end of the path. return original + return original_turn_signal_info; +} + void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * interface) const { lane_change_visitor_ = interface->get_debug_msg_array(); From 6a86265c0611e8fa93b637b2af696c5319b55a17 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 13 Jun 2023 16:57:18 +0900 Subject: [PATCH 05/15] fix(behavior_path_planner): not rest modules when new route with same uuid is recived (#3961) Signed-off-by: kosuke55 --- .../src/behavior_path_planner_node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 102ddac21edf8..c73c346fa87a8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1217,10 +1217,14 @@ void BehaviorPathPlannerNode::run() // update route const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); if (route_ptr) { + // uuid is not changed when rerouting with modified goal, + // in this case do not need to rest modules. + const bool has_same_route_id = + planner_data_->prev_route_id && route_ptr->uuid == planner_data_->prev_route_id; planner_data_->route_handler->setRoute(*route_ptr); // Reset behavior tree when new route is received, // so that the each modules do not have to care about the "route jump". - if (!is_first_time) { + if (!is_first_time && !has_same_route_id) { RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); #ifdef USE_OLD_ARCHITECTURE bt_manager_->resetBehaviorTree(); From 0eed93c9c581307c025be7205e42706283a97888 Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Tue, 13 Jun 2023 17:59:20 +0900 Subject: [PATCH 06/15] docs(behavior_velocity_intersection_module): fix link (#3962) Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --- planning/behavior_velocity_intersection_module/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 2c4ba7d3b3d5f..3ab642abe1e8f 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -18,7 +18,7 @@ This module is activated when the path contains the lanes with `turn_direction` ### Requirements/Limitations -- The HDMap needs to have the information of `turn_direction` tag (which should be one of `straight`, `left`, `right`) for all the lanes in intersections and `right_of_way` tag for specific lanes (refer to [RightOfWay](intersection-design.md#right-of-way) section for more details). See [lanelet2_extension document](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) for more detail. +- The HDMap needs to have the information of `turn_direction` tag (which should be one of `straight`, `left`, `right`) for all the lanes in intersections and `right_of_way` tag for specific lanes (refer to [RightOfWay](#right-of-way) section for more details). See [lanelet2_extension document](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) for more detail. - WIP(perception requirements/limitations) - WIP(sensor visibility requirements/limitations) @@ -50,7 +50,7 @@ This setting gives the following `attention_area` configurations. ### Target objects -For [stuck vehicle detection](intersection-design.md#stuck-vehicle-detection) and [collision detection](intersection-design.md#collision-detection), this module checks **car**, **bus**, **truck**, **trailer**, **motor cycle**, and **bicycle** type objects. +For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection](#collision-detection), this module checks **car**, **bus**, **truck**, **trailer**, **motor cycle**, and **bicycle** type objects. Objects that satisfy all of the following conditions are considered as target objects (possible collision objects): From e62422b5b6375585920d772526aaf71b96061880 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 13 Jun 2023 18:05:57 +0900 Subject: [PATCH 07/15] feat(crosswalk): support crosswalk regulatory element (#3939) * feat(crosswalk): use regulatory element Signed-off-by: satoshi-ota * feat(map_loader): show crosswalk areas Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../lanelet2_map_visualization_node.cpp | 10 +- .../src/manager.cpp | 144 ++++++++++++++---- .../src/manager.hpp | 16 +- .../src/scene_crosswalk.cpp | 51 +++++-- .../src/scene_crosswalk.hpp | 10 +- .../src/scene_walkway.cpp | 51 +++++-- .../src/scene_walkway.hpp | 8 +- .../src/util.cpp | 5 +- .../src/util.hpp | 2 +- 9 files changed, 232 insertions(+), 65 deletions(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index fad15948c368f..df845ca9bd252 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -111,6 +111,8 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::noStoppingAreas(all_lanelets); std::vector sb_reg_elems = lanelet::utils::query::speedBumps(all_lanelets); + std::vector cw_reg_elems = + lanelet::utils::query::crosswalks(all_lanelets); lanelet::ConstLineStrings3d parking_spaces = lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); @@ -126,8 +128,8 @@ void Lanelet2MapVisualizationNode::onMapBin( std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, - cl_no_stopping_areas, cl_no_obstacle_segmentation_area, + cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, + cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, cl_hatched_road_markings_line; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); @@ -142,6 +144,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); + setColor(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3); setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); @@ -185,6 +188,9 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); insertMarkerArray( &map_marker_array, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 37d30e4e386bf..bbe9d0efbc657 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -74,10 +74,19 @@ std::set getCrosswalkIdSetOnPath( return crosswalk_id_set; } + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + return !lanelet::utils::query::crosswalks(all_lanelets).empty(); +} } // namespace namespace behavior_velocity_planner { + +using lanelet::autoware::Crosswalk; + CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC(node, getModuleName()) { @@ -128,17 +137,43 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - for (const auto & crosswalk : getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr())) { - const auto module_id = crosswalk.id(); - if (!isModuleRegistered(module_id)) { - registerModule(std::make_shared( - module_id, crosswalk, crosswalk_planner_param_, logger_.get_child("crosswalk_module"), - clock_)); - generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); + if (!opt_use_regulatory_element_) { + opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); + std::ostringstream string_stream; + string_stream << "use crosswalk regulatory element: "; + string_stream << std::boolalpha << opt_use_regulatory_element_.get(); + RCLCPP_INFO_STREAM(logger_, string_stream.str()); + } + + const auto launch = [this, &path](const auto id) { + if (isModuleRegistered(id)) { + return; + } + + const auto & p = crosswalk_planner_param_; + const auto logger = logger_.get_child("crosswalk_module"); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + + registerModule(std::make_shared( + id, lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_)); + generateUUID(id); + updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); + }; + + if (opt_use_regulatory_element_.get()) { + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->id()); + } + } else { + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk.id()); } } } @@ -147,8 +182,21 @@ std::function &)> CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto crosswalk_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + std::set crosswalk_id_set; + + if (opt_use_regulatory_element_.get()) { + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + crosswalk_id_set.insert(crosswalk.first->id()); + } + } else { + crosswalk_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr()); + } return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; @@ -166,31 +214,69 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) wp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); } -void WalkwayModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - for (const auto & crosswalk : getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr())) { - const auto module_id = crosswalk.id(); - if ( - !isModuleRegistered(module_id) && - crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) == - lanelet::AttributeValueString::Walkway) { - registerModule(std::make_shared( - module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"), clock_)); + if (!opt_use_regulatory_element_) { + opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); + } + + const auto launch = [this, &path](const auto & lanelet) { + const auto attribute = + lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); + if (attribute != lanelet::AttributeValueString::Walkway) { + return; + } + + if (isModuleRegistered(lanelet.id())) { + return; + } + + const auto & p = walkway_planner_param_; + const auto logger = logger_.get_child("walkway_module"); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + + registerModule(std::make_shared( + lanelet.id(), lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_)); + }; + + if (opt_use_regulatory_element_.get()) { + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->crosswalkLanelet()); + } + } else { + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk); } } } std::function &)> -WalkwayModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + std::set walkway_id_set; + + if (opt_use_regulatory_element_.get()) { + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + walkway_id_set.insert(crosswalk.first->id()); + } + } else { + walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr()); + } return [walkway_id_set](const std::shared_ptr & scene_module) { return walkway_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 434c7eed9f065..024bb4582eb1a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,9 @@ namespace behavior_velocity_planner { + +using autoware_auto_planning_msgs::msg::PathWithLaneId; + class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC { public: @@ -43,10 +47,12 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC private: CrosswalkModule::PlannerParam crosswalk_planner_param_{}; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const PathWithLaneId & path) override; + + boost::optional opt_use_regulatory_element_{boost::none}; }; class WalkwayModuleManager : public SceneModuleManagerInterface @@ -59,10 +65,12 @@ class WalkwayModuleManager : public SceneModuleManagerInterface private: WalkwayModule::PlannerParam walkway_planner_param_{}; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const PathWithLaneId & path) override; + + boost::optional opt_use_regulatory_element_{boost::none}; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 6b2f15fe15719..7047fc7b56f2e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -127,15 +127,29 @@ void sortCrosswalksByDistance( } // namespace CrosswalkModule::CrosswalkModule( - const int64_t module_id, lanelet::ConstLanelet crosswalk, const PlannerParam & planner_param, + const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), - crosswalk_(std::move(crosswalk)), - planner_param_(planner_param) + planner_param_(planner_param), + use_regulatory_element_(use_regulatory_element) { velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; + + if (use_regulatory_element_) { + const auto reg_elem_ptr = std::dynamic_pointer_cast( + lanelet_map_ptr->regulatoryElementLayer.get(module_id)); + stop_lines_ = reg_elem_ptr->stopLines(); + crosswalk_ = reg_elem_ptr->crosswalkLanelet(); + } else { + const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id"); + if (!!stop_line) { + stop_lines_.push_back(stop_line.get()); + } + crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id); + } } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -239,20 +253,33 @@ boost::optional> CrosswalkModule::g { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); - exist_stopline_in_map = static_cast(stop_line); - if (stop_line) { + const auto get_stop_point = + [&](const auto & stop_line) -> boost::optional { const auto intersects = getLinestringIntersects( - ego_path, lanelet::utils::to2D(stop_line.get()).basicLineString(), ego_pos, 2); - if (!intersects.empty()) { - const auto p_stop_line = - createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z); - const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line); - return std::make_pair(dist_ego_to_stop, p_stop_line); + ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); + + if (intersects.empty()) { + return boost::none; + } + + return createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z); + }; + + for (const auto & stop_line : stop_lines_) { + const auto p_stop_line = get_stop_point(stop_line); + if (!p_stop_line) { + continue; } + + exist_stopline_in_map = true; + + const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line.get()); + return std::make_pair(dist_ego_to_stop, p_stop_line.get()); } { + exist_stopline_in_map = false; + if (!path_intersects_.empty()) { const auto p_stop_line = path_intersects_.front(); const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line) - diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 38ac0f009f785..c382d09e9ec6e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -18,6 +18,7 @@ #include "util.hpp" #include +#include #include #include #include @@ -52,6 +53,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::TrafficLight; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::StopWatch; @@ -93,7 +95,8 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - const int64_t module_id, lanelet::ConstLanelet crosswalk, const PlannerParam & planner_param, + const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -156,6 +159,8 @@ class CrosswalkModule : public SceneModuleInterface lanelet::ConstLanelet crosswalk_; + lanelet::ConstLineStrings3d stop_lines_; + std::vector path_intersects_; // Parameter @@ -173,6 +178,9 @@ class CrosswalkModule : public SceneModuleInterface // whether ego passed safety_slow_point bool passed_safety_slow_point_; + + // whether use regulatory element + bool use_regulatory_element_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp index ece8531e81f10..1265e73223eff 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp @@ -32,15 +32,29 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPose; WalkwayModule::WalkwayModule( - const int64_t module_id, lanelet::ConstLanelet walkway, const PlannerParam & planner_param, + const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), - walkway_(std::move(walkway)), state_(State::APPROACH), - planner_param_(planner_param) + planner_param_(planner_param), + use_regulatory_element_(use_regulatory_element) { velocity_factor_.init(VelocityFactor::SIDEWALK); + + if (use_regulatory_element_) { + const auto reg_elem_ptr = std::dynamic_pointer_cast( + lanelet_map_ptr->regulatoryElementLayer.get(module_id)); + stop_lines_ = reg_elem_ptr->stopLines(); + walkway_ = reg_elem_ptr->crosswalkLanelet(); + } else { + const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id"); + if (!!stop_line) { + stop_lines_.push_back(stop_line.get()); + } + walkway_ = lanelet_map_ptr->laneletLayer.get(module_id); + } } boost::optional> WalkwayModule::getStopLine( @@ -48,20 +62,33 @@ boost::optional> WalkwayModule::get { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); - exist_stopline_in_map = static_cast(stop_line); - if (stop_line) { + const auto get_stop_point = + [&](const auto & stop_line) -> boost::optional { const auto intersects = getLinestringIntersects( - ego_path, lanelet::utils::to2D(stop_line.get()).basicLineString(), ego_pos, 2); - if (!intersects.empty()) { - const auto p_stop_line = - createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z); - const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line); - return std::make_pair(dist_ego_to_stop, p_stop_line); + ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); + + if (intersects.empty()) { + return boost::none; + } + + return createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z); + }; + + for (const auto & stop_line : stop_lines_) { + const auto p_stop_line = get_stop_point(stop_line); + if (!p_stop_line) { + continue; } + + exist_stopline_in_map = true; + + const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line.get()); + return std::make_pair(dist_ego_to_stop, p_stop_line.get()); } { + exist_stopline_in_map = false; + if (!path_intersects_.empty()) { const auto p_stop_line = path_intersects_.front(); const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line) - diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp index 1479ef01c6b80..40ae4d9bbf1b0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp @@ -45,7 +45,8 @@ class WalkwayModule : public SceneModuleInterface double stop_duration_sec; }; WalkwayModule( - const int64_t module_id, lanelet::ConstLanelet walkway, const PlannerParam & planner_param, + const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const bool use_regulatory_element, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -63,6 +64,8 @@ class WalkwayModule : public SceneModuleInterface lanelet::ConstLanelet walkway_; + lanelet::ConstLineStrings3d stop_lines_; + std::vector path_intersects_; // State machine @@ -73,6 +76,9 @@ class WalkwayModule : public SceneModuleInterface // Debug DebugData debug_data_; + + // flag to use regulatory element + bool use_regulatory_element_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 9a32f96838a92..3c8d456ab8864 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -139,11 +139,10 @@ std::vector getLinestringIntersects( } lanelet::Optional getStopLineFromMap( - const int lane_id, const std::shared_ptr & planner_data, + const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name) { - lanelet::ConstLanelet lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); const auto road_markings = lanelet.regulatoryElementsAs(); lanelet::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { diff --git a/planning/behavior_velocity_crosswalk_module/src/util.hpp b/planning/behavior_velocity_crosswalk_module/src/util.hpp index 56c97b4d9f91a..10602b11c3c2e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.hpp @@ -81,7 +81,7 @@ std::vector> getLinestringIntersects( const geometry_msgs::msg::Point & ego_pos, const size_t max_num); lanelet::Optional getStopLineFromMap( - const int lane_id, const std::shared_ptr & planner_data, + const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name); } // namespace behavior_velocity_planner From e5e5b437c702c0d802b6e9635ff744da01d2c8f1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 13 Jun 2023 19:12:13 +0900 Subject: [PATCH 08/15] fix(goal_planenr): not reset when receving new route (#3966) * fix(goal_planenr): not reset when receving new route Signed-off-by: kosuke55 * fix for old archi Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 4 +- .../goal_planner/goal_planner_module.cpp | 41 ++++++++++++------- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 1156dd2dfc5e4..da2665723ae90 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -152,8 +152,10 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; - // save last time and pose +// save last time and pose +#ifdef USE_OLD_ARCHITECTURE std::unique_ptr last_received_time_; +#endif std::unique_ptr last_approved_time_; std::unique_ptr last_increment_time_; std::unique_ptr last_path_update_time_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 52a494ef659cc..09eeb0697c698 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -488,26 +488,37 @@ void GoalPlannerModule::returnToLaneParking() void GoalPlannerModule::generateGoalCandidates() { - // initialize when receiving new route const auto & route_handler = planner_data_->route_handler; + +// with old architecture, module instance is not cleared when new route is received +// so need to reset status here. +#ifdef USE_OLD_ARCHITECTURE + // initialize when receiving new route if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { - // Initialize parallel parking planner status + // this process causes twice reset when receiving first route. resetStatus(); - - // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); - refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (allow_goal_modification_) { - goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); - } else { - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = goal_pose; - goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); - } } last_received_time_ = std::make_unique(route_handler->getRouteHeader().stamp); + +#else + // todo: move this check out of this function after old architecture is removed + if (!goal_candidates_.empty()) { + return; + } +#endif + + // calculate goal candidates + const Pose goal_pose = route_handler->getGoalPose(); + refined_goal_pose_ = calcRefinedGoal(goal_pose); + if (allow_goal_modification_) { + goal_searcher_->setPlannerData(planner_data_); + goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + } else { + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = goal_pose; + goal_candidate.distance_from_original_goal = 0.0; + goal_candidates_.push_back(goal_candidate); + } } BehaviorModuleOutput GoalPlannerModule::plan() From 2eb0c7d0134bed2247384df6cc22a1c5f1630432 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Tue, 13 Jun 2023 14:01:48 +0300 Subject: [PATCH 09/15] chore: update CODEOWNERS (#3955) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3fab5b7090ca3..29d49c1a400ef 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -136,7 +136,7 @@ planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_speed_bump_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners From 57c958fe828759bda13c7f9f464b3884df13858a Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Tue, 13 Jun 2023 14:25:04 +0300 Subject: [PATCH 10/15] refactor(behavior_velocity_no_drivable_lane_module): relocate README file (#3967) Signed-off-by: Mehmet Dogru --- .../{docs/no-drivable-lane-design.md => README.md} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename planning/behavior_velocity_no_drivable_lane_module/{docs/no-drivable-lane-design.md => README.md} (96%) diff --git a/planning/behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md b/planning/behavior_velocity_no_drivable_lane_module/README.md similarity index 96% rename from planning/behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md rename to planning/behavior_velocity_no_drivable_lane_module/README.md index 57bff0f446259..eac481a0d733e 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md +++ b/planning/behavior_velocity_no_drivable_lane_module/README.md @@ -13,7 +13,7 @@ Some examples of No Drivable Lanes - Underpass road that goes under a railway, for safety reasons - Road with slope/inclination that the vehicle is not be able to drive autonomously due to technical limitations. And lots of other examples. -![no-drivable-lane-design.svg](no_drivable_lane_design.svg) +![no-drivable-lane-design.svg](./docs/no_drivable_lane_design.svg) A lanelet becomes invalid by adding a new tag under the relevant lanelet in the map file ``. @@ -42,7 +42,7 @@ This function is activated when the lane id of the target path has an no drivabl - the distance from front of the ego vehicle till the first intersection point between the ego path and the no drivable lane polygon is less than the `stop_margin` - Assign the state to `STOPPED` when the vehicle is completely stopped -![no_drivable_lane_scenarios.svg](no_drivable_lane_scenarios.svg) +![no_drivable_lane_scenarios.svg](./docs/no_drivable_lane_scenarios.svg) - At each state, RTC settings are assigned according to the following table From 751f128e7a8a6d224f6c44f235e130444eb81130 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 14 Jun 2023 09:37:30 +0900 Subject: [PATCH 11/15] fix(ekf_localizer): skip update of z/roll/pitch when mahalanobis gate enabled (#3915) * fix(ekf_localizer): skip update of z/roll/pitch when mahalanobis gate enabled Signed-off-by: kminoda * style(pre-commit): autofix * added basic test Signed-off-by: kminoda * fix small typo Signed-off-by: kminoda * style(pre-commit): autofix * better readability Signed-off-by: kminoda * style(pre-commit): autofix * now clinet works Signed-off-by: kminoda * Now tests work Signed-off-by: kminoda * remove unnecessary comments Signed-off-by: kminoda * fix copyright Signed-off-by: kminoda * style(pre-commit): autofix * fix pre-commit Signed-off-by: kminoda * fix pre-commit Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ekf_localizer/CMakeLists.txt | 8 + .../include/ekf_localizer/ekf_localizer.hpp | 3 +- localization/ekf_localizer/package.xml | 1 + .../ekf_localizer/src/ekf_localizer.cpp | 13 +- .../test/test_ekf_localizer_launch.py | 170 +++++++++++++ .../test/test_ekf_localizer_mahalanobis.py | 229 ++++++++++++++++++ 6 files changed, 417 insertions(+), 7 deletions(-) create mode 100644 localization/ekf_localizer/test/test_ekf_localizer_launch.py create mode 100644 localization/ekf_localizer/test/test_ekf_localizer_mahalanobis.py diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index bd9d22523ea06..6e7e194f7cf72 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -41,6 +41,14 @@ function(add_testcase filepath) endfunction() if(BUILD_TESTING) + add_ros_test( + test/test_ekf_localizer_launch.py + TIMEOUT "30" + ) + add_ros_test( + test/test_ekf_localizer_mahalanobis.py + TIMEOUT "30" + ) find_package(ament_cmake_gtest REQUIRED) set(TEST_FILES diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index d78d08b3f01bf..a4ae47b670897 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -254,7 +254,8 @@ class EKFLocalizer : public rclcpp::Node /** * @brief update simple1DFilter */ - void updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + void updateSimple1DFilters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); /** * @brief initialize simple1DFilter diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 4826b6daa17f1..8e837920f6d6a 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -33,6 +33,7 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + ros_testing ament_cmake diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index a30ccef5a39bb..651eca1e9de12 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -345,8 +345,6 @@ void EKFLocalizer::callbackPoseWithCovariance( } pose_queue_.push(msg); - - updateSimple1DFilters(*msg); } /* @@ -448,6 +446,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); ekf_.updateWithDelay(y, C, R, delay_step); + updateSimple1DFilters(pose, params_.pose_smoothing_steps); // debug const Eigen::MatrixXd X_result = ekf_.getLatestX(); @@ -600,16 +599,18 @@ void EKFLocalizer::publishEstimateResult() pub_debug_->publish(msg); } -void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void EKFLocalizer::updateSimple1DFilters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { double z = pose.pose.pose.position.z; const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - double z_dev = pose.pose.covariance[COV_IDX::Z_Z]; - double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL]; - double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH]; + double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast(smoothing_step); + double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast(smoothing_step); + double pitch_dev = + pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast(smoothing_step); z_filter_.update(z, z_dev, pose.header.stamp); roll_filter_.update(rpy.x, roll_dev, pose.header.stamp); diff --git a/localization/ekf_localizer/test/test_ekf_localizer_launch.py b/localization/ekf_localizer/test/test_ekf_localizer_launch.py new file mode 100644 index 0000000000000..f49388d783c12 --- /dev/null +++ b/localization/ekf_localizer/test/test_ekf_localizer_launch.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import time +import unittest + +from ament_index_python import get_package_share_directory +from geometry_msgs.msg import PoseWithCovarianceStamped +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +from nav_msgs.msg import Odometry +import pytest +import rclpy +from std_srvs.srv import SetBool + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_ekf_localizer_launch_file = os.path.join( + get_package_share_directory("ekf_localizer"), + "launch", + "ekf_localizer.launch.xml", + ) + ekf_localizer = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_ekf_localizer_launch_file), + ) + + return launch.LaunchDescription( + [ + ekf_localizer, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + + def tearDown(self): + self.test_node.destroy_node() + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Trigger ekf_localizer to activate the node + cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") + while not cli_trigger.wait_for_service(timeout_sec=1.0): + continue + + request = SetBool.Request() + request.data = True + future = cli_trigger.call_async(request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is not None: + self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) + else: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + + # Send initial pose + pub_init_pose = self.test_node.create_publisher( + PoseWithCovarianceStamped, "/initialpose3d", 10 + ) + init_pose = PoseWithCovarianceStamped() + init_pose.header.frame_id = "map" + init_pose.pose.pose.position.x = 0.0 + init_pose.pose.pose.position.y = 0.0 + init_pose.pose.pose.position.z = 0.0 + init_pose.pose.pose.orientation.x = 0.0 + init_pose.pose.pose.orientation.y = 0.0 + init_pose.pose.pose.orientation.z = 0.0 + init_pose.pose.pose.orientation.w = 1.0 + init_pose.pose.covariance = [ + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + ] + pub_init_pose.publish(init_pose) + + # Receive Odometry + msg_buffer = [] + self.test_node.create_subscription( + Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10 + ) + + # Wait until the node publishes some topic + end_time = time.time() + self.evaluation_time + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Check if the EKF outputs some Odometry + self.assertTrue(len(msg_buffer) > 0) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/ekf_localizer/test/test_ekf_localizer_mahalanobis.py b/localization/ekf_localizer/test/test_ekf_localizer_mahalanobis.py new file mode 100644 index 0000000000000..4e95ac142e524 --- /dev/null +++ b/localization/ekf_localizer/test/test_ekf_localizer_mahalanobis.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import time +import unittest + +from ament_index_python import get_package_share_directory +from geometry_msgs.msg import PoseWithCovarianceStamped +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +from nav_msgs.msg import Odometry +import pytest +import rclpy +from std_srvs.srv import SetBool + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_ekf_localizer_launch_file = os.path.join( + get_package_share_directory("ekf_localizer"), + "launch", + "ekf_localizer.launch.xml", + ) + ekf_localizer = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_ekf_localizer_launch_file), + ) + + return launch.LaunchDescription( + [ + ekf_localizer, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + + def tearDown(self): + self.test_node.destroy_node() + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Trigger ekf_localizer to activate the node + cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") + while not cli_trigger.wait_for_service(timeout_sec=1.0): + continue + + request = SetBool.Request() + request.data = True + future = cli_trigger.call_async(request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is not None: + self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) + else: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + + # Send initial pose + pub_init_pose = self.test_node.create_publisher( + PoseWithCovarianceStamped, "/initialpose3d", 10 + ) + init_pose = PoseWithCovarianceStamped() + init_pose.header.frame_id = "map" + init_pose.pose.pose.position.x = 0.0 + init_pose.pose.pose.position.y = 0.0 + init_pose.pose.pose.position.z = 0.0 + init_pose.pose.pose.orientation.x = 0.0 + init_pose.pose.pose.orientation.y = 0.0 + init_pose.pose.pose.orientation.z = 0.0 + init_pose.pose.pose.orientation.w = 1.0 + init_pose.pose.covariance = [ + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + ] + pub_init_pose.publish(init_pose) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Send pose that should be ignored by mahalanobis gate in ekf_localizer + pub_pose = self.test_node.create_publisher( + PoseWithCovarianceStamped, "/in_pose_with_covariance", 10 + ) + pose = PoseWithCovarianceStamped() + pose.header.frame_id = "map" + pose.pose.pose.position.x = 1000000.0 + pose.pose.pose.position.y = 1000000.0 + pose.pose.pose.position.z = 10.0 + pose.pose.pose.orientation.x = 0.0 + pose.pose.pose.orientation.y = 0.0 + pose.pose.pose.orientation.z = 0.0 + pose.pose.pose.orientation.w = 1.0 + pose.pose.covariance = [ + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + ] + pub_pose.publish(pose) + + # Receive Odometry + msg_buffer = [] + self.test_node.create_subscription( + Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10 + ) + + # Wait until the node publishes some topic + end_time = time.time() + self.evaluation_time + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Check if the EKF outputs some Odometry + self.assertTrue(len(msg_buffer) > 0) + + # Assert msg to be at the origin + self.assertEqual(msg_buffer[-1].pose.pose.position.x, 0.0) + self.assertEqual(msg_buffer[-1].pose.pose.position.y, 0.0) + self.assertEqual(msg_buffer[-1].pose.pose.position.z, 0.0) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From cbc51c1aba7cddd4213fbee1de9486a7a31adbb2 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 14 Jun 2023 13:52:10 +0900 Subject: [PATCH 12/15] docs(behavior_path_planner): add explanation for dynamic lateral acceleration (#3965) * docs(behavior_path_planner): add explanation for dynamic lateral acceleration Signed-off-by: yutaka * udpate Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- ...ehavior_path_planner_lane_change_design.md | 37 +++++++++++++++---- 1 file changed, 29 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 39b67c32edb70..945ca579c434a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -38,27 +38,27 @@ lane_changing_distance = lane_change_prepare_velocity * lane_changing_duration The `backward_length_buffer_for_end_of_lane` is added to allow some window for any possible delay, such as control or mechanical delay during brake lag. -#### Multiple candidate path samples +#### Multiple candidate path samples (longitudinal acceleration) Lane change velocity is affected by the ego vehicle's current velocity. High velocity requires longer preparation and lane changing distance. However we also need to plan lane changing trajectories in case ego vehicle slows down. Computing candidate paths that assumes ego vehicle's slows down is performed by substituting predetermined deceleration value into `prepare_length`, `prepare_velocity` and `lane_changing_length` equation. -The predetermined acceleration values are a set of value that starts from `acceleration = maximum_acceleration`, and decrease by `acceleration_resolution` until it reaches `acceleration = -maximum_deceleration`. Both `maximum_acceleration` and `maximum_deceleration` are calculated as: defined in the `common.param` file as `normal.min_acc`. +The predetermined longitudinal acceleration values are a set of value that starts from `longitudinal_acceleration = maximum_longitudinal_acceleration`, and decrease by `longitudinal_acceleration_resolution` until it reaches `longitudinal_acceleration = -maximum_longitudinal_deceleration`. Both `maximum_longitudinal_acceleration` and `maximum_longitudinal_deceleration` are calculated as: defined in the `common.param` file as `normal.min_acc`. ```C++ -maximum_acceleration = min(common_param.max_acc, lane_change_param.max_acc) -maximum_deceleration = max(common_param.min_acc, lane_change_param.min_acc) +maximum_longitudinal_acceleration = min(common_param.max_acc, lane_change_param.max_acc) +maximum_longitudinal_deceleration = max(common_param.min_acc, lane_change_param.min_acc) ``` -where `common_param` is vehicle common parameter, which defines vehicle common maximum acceleration and deceleration. Whereas, `lane_change_param` has maximum acceleration and deceleration for the lane change module. For example, if a user set and `common_param.max_acc=1.0` and `lane_change_param.max_acc=0.0`, `maximum_acceleration` becomes `0.0`, and the lane change does not accelerate in the lane change phase. +where `common_param` is vehicle common parameter, which defines vehicle common maximum longitudinal acceleration and deceleration. Whereas, `lane_change_param` has maximum longitudinal acceleration and deceleration for the lane change module. For example, if a user set and `common_param.max_acc=1.0` and `lane_change_param.max_acc=0.0`, `maximum_longitudinal_acceleration` becomes `0.0`, and the lane change does not accelerate in the lane change phase. -The `acceleration_resolution` is determine by the following +The `longitudinal_acceleration_resolution` is determine by the following ```C++ -acceleration_resolution = (maximum_acceleration - maximum_deceleration) / lane_change_sampling_num +longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_deceleration) / longitudinal_acceleration_sampling_num ``` -Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, the acceleration becomes positive value (not deceleration). +Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate). The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. @@ -66,6 +66,27 @@ The following figure illustrates when `lane_change_sampling_num = 4`. Assuming t Which path will be chosen will depend on validity and collision check. +#### Multiple candidate path samples (lateral acceleration) + +In addition to sampling longitudinal acceleration, we also sample lane change paths by adjusting the value of lateral acceleration. Since lateral acceleration influences the duration of a lane change, a lower lateral acceleration value results in a longer lane change path, while a higher lateral acceleration value leads to a shorter lane change path. This allows the lane change module to generate a shorter lane change path by increasing the lateral acceleration when there is limited space for the lane change. + +The maximum and minimum lateral accelerations are defined in the lane change parameter file as a map. The range of lateral acceleration is determined for each velocity by linearly interpolating the values in the map. Let's assume we have the following map + +| Ego Velocity | Minimum lateral acceleration | Maximum lateral acceleration | +| :----------- | ---------------------------- | ---------------------------- | +| 0.0 | 0.2 | 0.3 | +| 2.0 | 0.2 | 0.4 | +| 4.0 | 0.3 | 0.4 | +| 6.0 | 0.3 | 0.5 | + +In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.2 and 0.35 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. + +Within this range, we sample the lateral acceleration for the ego vehicle. Similar to the method used for sampling longitudinal acceleration, the resolution of lateral acceleration (lateral_acceleration_resolution) is determined by the following: + +```C++ +lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_deceleration) / lateral_acceleration_sampling_num +``` + #### Candidate Path's validity check A candidate path is valid if the total lane change distance is less than From e1be8ccb5d701943b9972d188180a639937fdd23 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 14 Jun 2023 15:07:24 +0900 Subject: [PATCH 13/15] fix(behavior_path_planner): fix link in path shifter document (#3974) Signed-off-by: Fumiya Watanabe --- .../docs/behavior_path_planner_path_generation_design.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md index 2b0de87ae2944..7784f96543ea6 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md @@ -8,9 +8,7 @@ The base idea of the path generation in lane change and avoidance is to smoothly The figure below explains how the application of a constant lateral jerk $l^{'''}(s)$ can be used to induce lateral shifting. In order to comply with the limits on lateral acceleration and velocity, zero-jerk time is employed in the figure ( $T_a$ and $T_v$ ). In each interval where constant jerk is applied, the shift position $l(s)$ can be characterized by a third-degree polynomial. Therefore the shift length from the reference path can then be calculated by combining spline curves. -

- -

+![path-shifter](../image/path_shifter.png) Note that, due to the rarity of the $T_v$ in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. From 3a1c1148c4438641e77c0bb610cfd99421c38e33 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 14 Jun 2023 15:22:17 +0900 Subject: [PATCH 14/15] fix(start_planner): fix start planner is_stopped (#3973) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 3564c7a63f4b7..1eea0aa35e5e1 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -127,7 +127,7 @@ bool StartPlannerModule::isExecutionRequested() const } const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < - parameters_->th_arrived_distance; + parameters_->th_stopped_velocity; if (!is_stopped) { #ifdef USE_OLD_ARCHITECTURE is_executed_ = false; From b8ddaf55f0b262d88070a415612c39975420a6dd Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 14 Jun 2023 15:22:26 +0900 Subject: [PATCH 15/15] fix(start_planner): fix start planner pendulum (#3972) Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 1eea0aa35e5e1..3e934206aeb3a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -105,6 +105,18 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { + // Check if ego arrives at goal + const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + if ( + tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < + parameters_->th_arrived_distance) { +#ifdef USE_OLD_ARCHITECTURE + is_executed_ = false; +#endif + return false; + } + has_received_new_route_ = !planner_data_->prev_route_id || *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();