diff --git a/.cspell-partial.json b/.cspell-partial.json index a3c4ca455d444..45b85e779c39d 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,10 +1,5 @@ { - "ignorePaths": [ - "**/perception/**", - "**/planning/**", - "**/sensing/**", - "perception/bytetrack/lib/**" - ], + "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"], "ignoreRegExpList": [], "words": [] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 4b1727e8a67b1..fb6df15ab7679 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -15,6 +15,7 @@ common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp m common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp +common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @@ -53,9 +54,10 @@ common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/lane_departure_checker/** kyoichi.sugahara@tier4.jp +control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @@ -82,7 +84,6 @@ launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/initial_pose_button_panel/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -183,11 +184,10 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -sensing/geo_pos_conv/** yamato.ando@tier4.jp sensing/gnss_poser/** yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp -sensing/imu_corrector/** yamato.ando@tier4.jp +sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp 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 b793173285629..fddd535931d94 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 @@ -163,6 +163,8 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](../image/lane_change/lane_objects.drawio.svg) +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. + ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. @@ -177,21 +179,13 @@ When driving on the public road with other vehicles, there exist scenarios where ```C++ lane_changing_time = f(shift_length, lat_acceleration, lat_jerk) -minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + backward_length_buffer_for_end_of_lane +minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + lane_change_finish_judge_buffer ``` The following figure illustrates when the lane is blocked in multiple lane changes cases. ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) -#### Intersection - -Lane change in the intersection is prohibited following traffic regulation. Therefore, if the goal is place close passed the intersection, the lane change needs to be completed before ego vehicle enters the intersection region. Similar to the lane blocked case, in case of the path is unsafe, ego vehicle will stop and waits for the dynamic object to pass by. - -The following figure illustrate the intersection case. - -![intersection](../image/lane_change/lane_change-intersection_case.png) - ### Aborting lane change The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. @@ -215,11 +209,11 @@ while(Lane Following) if (Is Abort Condition Satisfied) then (**NO**) else (**YES**) if (Is Enough margin to retry lane change) then (**YES**) - if (Ego not depart from current lane yet) then (**YES**) + if (Ego is on lane change prepare phase) then (**YES**) :Cancel lane change; break else (**NO**) - if (Can perform abort maneuver) then (**YES**) + if (Will the overhang to target lane be less than threshold?) then (**YES**) :Perform abort maneuver; break else (NO) @@ -270,23 +264,40 @@ The last behavior will also occur if the ego vehicle has departed from the curre The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------- | ------------- | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------ | ------ | ------- | --------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | +| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `object_check_min_road_shoulder_width` | [m] | double | Vehicles around the center line within this distance will be excluded from parking objects | 0.5 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | +| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | ### Collision checks during lane change -The following parameters are configurable in `behavior_path_planner.param.yaml`. +The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. | Name | Unit | Type | Description | Default value | | :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index fcde58756d4be..7889e91767155 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -513,6 +513,12 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); + if (!path_shifter_.getShiftLines().empty()) { + left_shift_array_.clear(); + right_shift_array_.clear(); + removeRTCStatus(); + } + current_raw_shift_lines_.clear(); registered_raw_shift_lines_.clear(); path_shifter_.setShiftLines(ShiftLineArray{}); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index b0fda03418069..70999f1504fee 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -602,7 +602,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif case AvoidanceState::YIELD: { insertYieldVelocity(path); insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - removeRegisteredShiftLines(); break; } case AvoidanceState::AVOID_PATH_NOT_READY: { @@ -2115,6 +2114,10 @@ BehaviorModuleOutput AvoidanceModule::plan() updatePathShifter(data.safe_new_sl); + if (data.yield_required) { + removeRegisteredShiftLines(); + } + // generate path with shift points that have been inserted. ShiftedPath linear_shift_path = utils::avoidance::toShiftedPath(data.reference_path); ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); @@ -2546,12 +2549,14 @@ void AvoidanceModule::updateRTCData() updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); - if (data.safe_new_sl.empty()) { + const auto candidates = data.safe ? data.safe_new_sl : data.unapproved_new_sl; + + if (candidates.empty()) { removeCandidateRTCStatus(); return; } - const auto shift_line = helper_.getMainShiftLine(data.safe_new_sl); + const auto shift_line = helper_.getMainShiftLine(candidates); if (helper_.getRelativeShiftToPath(shift_line) > 0.0) { removePreviousRTCStatusRight(); } else if (helper_.getRelativeShiftToPath(shift_line) < 0.0) { @@ -2562,8 +2567,8 @@ void AvoidanceModule::updateRTCData() CandidateOutput output; - const auto sl_front = data.safe_new_sl.front(); - const auto sl_back = data.safe_new_sl.back(); + const auto sl_front = candidates.front(); + const auto sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; output.lateral_shift = helper_.getRelativeShiftToPath(shift_line); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 8fdb79a6271d1..3db3e049c3c81 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -645,10 +645,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto & objects = *planner_data_->dynamic_object; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters_->object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters_->object_shiftable_ratio_threshold; // Guard if (objects.objects.empty()) { @@ -709,9 +705,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // check if the object intersects with target lanes if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { // ignore static parked object that are in front of the ego vehicle in target lanes - bool is_parked_object = utils::lane_change::isParkedObject( - target_path, route_handler, extended_object, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); + bool is_parked_object = + utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { continue; } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 6da5ff97b9bf8..eb2859ff700af 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1795,8 +1795,22 @@ void makeBoundLongitudinallyMonotonic( continue; } - for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { - set_orientation(ret, j, getPose(path_points.at(i)).orientation); + if (i + 1 == path_points.size()) { + for (size_t j = intersect_idx.get(); j < bound_with_pose.size(); j++) { + if (j + 1 == bound_with_pose.size()) { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } else { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j).position, bound_with_pose.at(j + 1).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } + } + } else { + for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + set_orientation(ret, j, getPose(path_points.at(i)).orientation); + } } constexpr size_t OVERLAP_CHECK_NUM = 3; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index d9ebafbe682a0..7ae549ce8b274 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1166,7 +1166,7 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, + const std::vector & lane_divisions, const std::vector & parked_attention_objects, const double occlusion_dist_thr) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 9d1fab1339d0e..774b68710be7f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -219,7 +219,7 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; + std::optional> occlusion_attention_divisions_; // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop @@ -267,7 +267,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, + const std::vector & lane_divisions, const std::vector & parked_attention_objects, const double occlusion_dist_thr); diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 7fdae3df2d9d2..a987ce9af9dfe 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -709,7 +709,7 @@ bool isTrafficLightArrowActivated( return false; } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) @@ -778,15 +778,15 @@ std::vector generateDetectionLaneDivisions( auto & branch = branches[(ind2id[src])]; int node_iter = ind2id[src]; while (true) { - const auto & dsts = adjacency[(id2ind[node_iter])]; + const auto & destinations = adjacency[(id2ind[node_iter])]; // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(dsts.begin(), dsts.end(), true); - if (next == dsts.end()) { + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { branch.push_back(node_iter); break; } branch.push_back(node_iter); - node_iter = ind2id[std::distance(dsts.begin(), next)]; + node_iter = ind2id[std::distance(destinations.begin(), next)]; } } for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { @@ -818,9 +818,9 @@ std::vector generateDetectionLaneDivisions( } // (3) discretize each merged lanelet - std::vector detection_divisions; + std::vector detection_divisions; for (const auto & [last_lane_id, branch] : merged_branches) { - DescritizedLane detection_division; + DiscretizedLane detection_division; detection_division.lane_id = last_lane_id; const auto detection_lanelet = branch.first; const double area = branch.second; @@ -1118,14 +1118,14 @@ void IntersectionLanelets::update( static lanelet::ConstLanelets getPrevLanelets( const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) { - lanelet::ConstLanelets prevs; + lanelet::ConstLanelets previous_lanelets; for (const auto & ll : lanelets_on_path) { if (associative_ids.find(ll.id()) != associative_ids.end()) { - return prevs; + return previous_lanelets; } - prevs.push_back(ll); + previous_lanelets.push_back(ll); } - return prevs; + return previous_lanelets; } std::optional generatePathLanelets( diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e125f485a65e8..287a75979927b 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -114,7 +114,7 @@ bool isTrafficLightArrowActivated( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4d2cf9695b2db..26187d75ff53c 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -115,7 +115,7 @@ struct IntersectionLanelets std::optional first_attention_area_ = std::nullopt; }; -struct DescritizedLane +struct DiscretizedLane { int lane_id; // discrete fine lines from left to right @@ -135,7 +135,7 @@ struct IntersectionStopLines struct PathLanelets { lanelet::ConstLanelets prev; - // lanelet::Constlanelet entry2ego; this is included in `all` if exists + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists lanelet::ConstLanelet ego_or_entry2exit; // this is `assigned lane` part of the path(not from // ego) if ego is before the intersection, otherwise from ego to exit diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 381b8b472adfa..afe345490201b 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -350,6 +350,7 @@ bool CostmapGenerator::isActive() return false; } else { const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger()); + if (!current_pose_wrt_map) return false; return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose); } } diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml index 78bddc2b81f4f..86f767560fd7c 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -13,13 +13,13 @@ - + - + diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 3d72d6e4b5c83..2bcb31ff10969 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -222,38 +222,38 @@ bool calcStopVelocityWithConstantJerkAccLimit( return true; } - double dist = 0.0; - std::vector dists; - dists.push_back(dist); + double distance = 0.0; + std::vector distances; + distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - dist += + distance += tier4_autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); - if (dist > xs.back()) { + if (distance > xs.back()) { break; } - dists.push_back(dist); + distances.push_back(distance); } if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(dists) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(dists)) { + !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distances) || + !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distances)) { return false; } if ( - xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || dists.empty() || - dists.front() < xs.front() || xs.back() < dists.back()) { + xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || distances.empty() || + distances.front() < xs.front() || xs.back() < distances.back()) { return false; } - const auto vel_at_wp = interpolation::lerp(xs, vs, dists); - const auto acc_at_wp = interpolation::lerp(xs, as, dists); - const auto jerk_at_wp = interpolation::lerp(xs, js, dists); + const auto vel_at_wp = interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = interpolation::lerp(xs, js, distances); // for debug std::stringstream ssi; - for (unsigned int i = 0; i < dists.size(); ++i) { - ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) + for (unsigned int i = 0; i < distances.size(); ++i) { + ssi << "d: " << distances.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) << ", j: " << jerk_at_wp.at(i) << std::endl; } RCLCPP_DEBUG( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 1927b266b0186..260a4c6400588 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -235,7 +235,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node } void reset(const std::string & uuid) { counter_.emplace(uuid, 0); } - // NOTE: positive is for meeting entrying condition, and negative is for exiting. + // NOTE: positive is for meeting entering condition, and negative is for exiting. std::unordered_map counter_; std::vector current_uuids_; }; diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index bd067e00e4c4e..07c6cfd9a570d 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -5,6 +5,8 @@ 1.0.0 The ROS 2 imu_corrector package Yamato Ando + Taiki Yamada + Koji Minoda Apache License 2.0 Yamato Ando