diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000000000..e6f359f7389aa --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,159 @@ +### Copied from .github/CODEOWNERS-manual ### +.github/* @autowarefoundation/autoware-maintainers + +### Automatically generated from package.xml ### +control/external_cmd_selector/* kenji.miyake@tier4.jp +control/obstacle_collision_checker/* kenji.miyake@tier4.jp +control/vehicle_cmd_gate/* takamasa.horibe@tier4.jp +control/trajectory_follower/* takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/trajectory_follower_nodes/* takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/joy_controller/* kenji.miyake@tier4.jp +control/lane_departure_checker/* kyoichi.sugahara@tier4.jp +control/shift_decider/* takamasa.horibe@tier4.jp +control/pure_pursuit/* takamasa.horibe@tier4.jp +control/operation_mode_transition_manager/* takamasa.horibe@tier4.jp +control/control_performance_analysis/* ali.boyali@tier4.jp berkay@leodrive.ai +common/autoware_ad_api_msgs/* isamu.takagi@tier4.jp +common/motion_common/* christopherj.ho@gmail.com +common/component_interface_specs/* isamu.takagi@tier4.jp +common/motion_testing/* christopherj.ho@gmail.com +common/autoware_auto_perception_rviz_plugin/* opensource@apex.ai taichi.higashide@tier4.jp +common/autoware_auto_tf2/* jit.ray.c@gmail.com +common/signal_processing/* takayuki.murooka@tier4.jp +common/tier4_perception_rviz_plugin/* yukihiro.saito@tier4.jp +common/path_distance_calculator/* isamu.takagi@tier4.jp +common/kalman_filter/* yukihiro.saito@tier4.jp +common/tier4_traffic_light_rviz_plugin/* satoshi.ota@tier4.jp +common/vehicle_constants_manager/* mfc@leodrive.ai +common/component_interface_utils/* isamu.takagi@tier4.jp +common/tier4_planning_rviz_plugin/* yukihiro.saito@tier4.jp +common/perception_utils/* takayuki.murooka@tier4.jp +common/web_controller/* yukihiro.saito@tier4.jp +common/goal_distance_calculator/* taiki.tanaka@tier4.jp +common/motion_utils/* yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +common/tier4_debug_tools/* kenji.miyake@tier4.jp +common/tier4_localization_rviz_plugin/* isamu.takagi@tier4.jp +common/autoware_point_types/* taichi.higashide@tier4.jp +common/tier4_screen_capture_rviz_plugin/* taiki.tanaka@tier4.jp +common/tier4_control_rviz_plugin/* taiki.tanaka@tier4.jp +common/osqp_interface/* maxime.clement@tier4.jp +common/tier4_simulated_clock_rviz_plugin/* maxime.clement@tier4.jp +common/neural_networks_provider/* ambroise.vincent@arm.com +common/global_parameter_loader/* kenji.miyake@tier4.jp +common/autoware_auto_common/* opensource@apex.ai +common/tvm_utility/* ambroise.vincent@arm.com +common/autoware_auto_geometry/* opensource@apex.ai +common/autoware_ad_api_specs/* isamu.takagi@tier4.jp +common/tier4_state_rviz_plugin/* hiroki.ota@tier4.jp +common/tier4_datetime_rviz_plugin/* isamu.takagi@tier4.jp +common/grid_map_utils/* maxime.clement@tier4.jp +common/tier4_autoware_utils/* takamasa.horibe@tier4.jp kenji.miyake@tier4.jp +common/fake_test_node/* opensource@apex.ai +common/autoware_testing/* adam.dabrowski@robotec.ai +common/tier4_api_utils/* isamu.takagi@tier4.jp +common/tier4_calibration_rviz_plugin/* tomoya.kimura@tier4.jp +common/interpolation/* fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp +common/time_utils/* christopherj.ho@gmail.com +common/polar_grid/* yukihiro.saito@tier4.jp +common/tier4_vehicle_rviz_plugin/* yukihiro.saito@tier4.jp +launch/tier4_vehicle_launch/* yukihiro.saito@tier4.jp +launch/tier4_system_launch/* kenji.miyake@tier4.jp +launch/tier4_map_launch/* ryohsuke.mitsudome@tier4.jp +launch/tier4_control_launch/* takamasa.horibe@tier4.jp +launch/tier4_sensing_launch/* yukihiro.saito@tier4.jp +launch/tier4_autoware_api_launch/* isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp +launch/tier4_perception_launch/* yukihiro.saito@tier4.jp +launch/tier4_planning_launch/* takamasa.horibe@tier4.jp +launch/tier4_localization_launch/* yamato.ando@tier4.jp +launch/tier4_simulator_launch/* keisuke.shima@tier4.jp +evaluator/localization_evaluator/* dominik.jargot@robotec.ai +evaluator/kinematic_evaluator/* dominik.jargot@robotec.ai +map/map_loader/* ryohsuke.mitsudome@tier4.jp kenji.miyake@tier4.jp +map/map_tf_generator/* azumi.suzuki@tier4.jp +map/util/lanelet2_map_preprocessor/* ryohsuke.mitsudome@tier4.jp +simulator/simple_planning_simulator/* takamasa.horibe@tier4.jp +simulator/dummy_perception_publisher/* yukihiro.saito@tier4.jp +simulator/fault_injection/* keisuke.shima@tier4.jp +tools/simulator_test/simulator_compatibility_test/* shpark@morai.ai +sensing/livox/livox_tag_filter/* kenji.miyake@tier4.jp +sensing/pointcloud_preprocessor/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +sensing/probabilistic_occupancy_grid_map/* yukihiro.saito@tier4.jp +sensing/imu_corrector/* yamato.ando@tier4.jp +sensing/geo_pos_conv/* yamato.ando@tier4.jp +sensing/image_transport_decompressor/* yukihiro.saito@tier4.jp +sensing/vehicle_velocity_converter/* ryu.yamamoto@tier4.jp +sensing/image_diagnostics/* dai.nguyen@tier4.jp +sensing/tier4_pcl_extensions/* ryu.yamamoto@tier4.jp +sensing/gnss_poser/* yamato.ando@tier4.jp +planning/freespace_planner/* takamasa.horibe@tier4.jp +planning/obstacle_cruise_planner/* takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/planning_error_monitor/* yutaka.shimizu@tier4.jp +planning/surround_obstacle_checker/* satoshi.ota@tier4.jp +planning/costmap_generator/* kenji.miyake@tier4.jp +planning/behavior_path_planner/* zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp +planning/obstacle_stop_planner/* satoshi.ota@tier4.jp +planning/behavior_velocity_planner/* mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp +planning/rtc_auto_approver/* fumiya.watanabe@tier4.jp +planning/scenario_selector/* kenji.miyake@tier4.jp +planning/planning_evaluator/* maxime.clement@tier4.jp +planning/rtc_interface/* fumiya.watanabe@tier4.jp +planning/planning_debug_tools/* takamasa.horibe@tier4.jp +planning/external_velocity_limit_selector/* satoshi.ota@tier4.jp +planning/freespace_planning_algorithms/* takamasa.horibe@tier4.jp +planning/obstacle_avoidance_planner/* takayuki.murooka@tier4.jp +planning/motion_velocity_smoother/* fumiya.watanabe@tier4.jp +planning/route_handler/* fumiya.watanabe@tier4.jp +planning/mission_planner/* ryohsuke.mitsudome@tier4.jp +perception/shape_estimation/* yukihiro.saito@tier4.jp +perception/front_vehicle_velocity_estimator/* satoshi.tanaka@tier4.jp +perception/traffic_light_visualization/* yukihiro.saito@tier4.jp +perception/detection_by_tracker/* yukihiro.saito@tier4.jp +perception/object_range_splitter/* yukihiro.saito@tier4.jp +perception/heatmap_visualizer/* kotaro.uetake@tier4.jp +perception/detected_object_feature_remover/* tomoya.kimura@tier4.jp +perception/crosswalk_traffic_light_estimator/* satoshi.ota@tier4.jp +perception/euclidean_cluster/* yukihiro.saito@tier4.jp +perception/image_projection_based_fusion/* yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/traffic_light_ssd_fine_detector/* daisuke.nishimatsu@tier4.jp +perception/lidar_centerpoint/* yusuke.muramatsu@tier4.jp +perception/map_based_prediction/* yutaka.shimizu@tier4.jp +perception/multi_object_tracker/* yukihiro.saito@tier4.jp jilada.eccleston@tier4.jp +perception/object_merger/* yukihiro.saito@tier4.jp +perception/elevation_map_loader/* kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp +perception/traffic_light_map_based_detector/* yukihiro.saito@tier4.jp +perception/tensorrt_yolo/* daisuke.nishimatsu@tier4.jp +perception/lidar_apollo_instance_segmentation/* yukihiro.saito@tier4.jp +perception/radar_fusion_to_detected_object/* satoshi.tanaka@tier4.jp +perception/radar_tracks_msgs_converter/* satoshi.tanaka@tier4.jp +perception/traffic_light_classifier/* yukihiro.saito@tier4.jp +perception/detected_object_validation/* yukihiro.saito@tier4.jp +perception/ground_segmentation/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +perception/occupancy_grid_map_outlier_filter/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +perception/compare_map_segmentation/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +vehicle/steer_offset_estimator/* taiki.tanaka@tier4.jp +vehicle/raw_vehicle_cmd_converter/* takamasa.horibe@tier4.jp +vehicle/vehicle_info_util/* yamato.ando@tier4.jp +vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/* tomoya.kimura@tier4.jp +vehicle/external_cmd_converter/* takamasa.horibe@tier4.jp +localization/pose_initializer/* yamato.ando@tier4.jp +localization/ndt_pcl_modified/* yamato.ando@tier4.jp +localization/pose2twist/* yamato.ando@tier4.jp +localization/ndt_scan_matcher/* yamato.ando@tier4.jp +localization/gyro_odometer/* yamato.ando@tier4.jp +localization/localization_error_monitor/* yamato.ando@tier4.jp +localization/initial_pose_button_panel/* yamato.ando@tier4.jp +localization/twist2accel/* koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/ndt/* yamato.ando@tier4.jp +localization/ekf_localizer/* takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/* koji.minoda@tier4.jp yamato.ando@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/* isamu.takagi@tier4.jp +system/velodyne_monitor/* fumihito.ito@tier4.jp +system/dummy_infrastructure/* kenji.miyake@tier4.jp +system/topic_state_monitor/* kenji.miyake@tier4.jp +system/bluetooth_monitor/* fumihito.ito@tier4.jp +system/ad_service_state_monitor/* kenji.miyake@tier4.jp +system/system_monitor/* fumihito.ito@tier4.jp +system/emergency_handler/* kenji.miyake@tier4.jp +system/dummy_diag_publisher/* kenji.miyake@tier4.jp +system/system_error_monitor/* kenji.miyake@tier4.jp +system/default_ad_api/* isamu.takagi@tier4.jp diff --git a/.yamllint.yaml b/.yamllint.yaml index 6228c70f02da9..2c7bd088e2648 100644 --- a/.yamllint.yaml +++ b/.yamllint.yaml @@ -1,7 +1,6 @@ extends: default ignore: | - .clang-tidy *.param.yaml rules: diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index eb07deaa2ce23..0ea817ffefc81 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 19b22ca850cbc..870013f45466a 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -460,8 +460,8 @@ std::vector GeometricParallelParking::planOneTrial( // debug Cr_.pose = Cr; Cr_.header = planner_data_->route_handler->getRouteHeader(); - Cr_.pose = Cr; - Cr_.header = planner_data_->route_handler->getRouteHeader(); + Cl_.pose = Cl; + Cl_.header = planner_data_->route_handler->getRouteHeader(); start_pose_.pose = start_pose; start_pose_.header = planner_data_->route_handler->getRouteHeader(); arc_end_pose_.pose = arc_end_pose; diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index eb07deaa2ce23..0ea817ffefc81 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 1879b7178e120..bffd25f1f5002 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -105,6 +105,10 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double keep_detection_line_margin; //! distance (toward path end) from generated stop line. + //! keep detection if ego is before this line and ego.vel < + //! keep_detection_vel_thr + double keep_detection_vel_thr; double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index e228deec9d704..94a5d2fac2095 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -39,7 +39,7 @@ int insertPoint( autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx); @@ -54,6 +54,14 @@ bool getObjectiveLanelets( std::vector * objective_lanelets_with_margin_result, const rclcpp::Logger logger); +struct StopLineIdx +{ + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; +}; + /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, * read it from the map; otherwise, generate a stop line at a position where it will not collide. @@ -68,9 +76,10 @@ bool getObjectiveLanelets( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger); /** * @brief If use_stuck_stopline is true, a stop line is generated before the intersection. @@ -118,6 +127,22 @@ std::vector getLaneletIdsFromLaneletsVec(const std::vectorresampleTrajectory( - *traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits::max()); + *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( *traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index f4f389ad5e45a..df4cfc2051056 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -34,6 +34,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + ip.keep_detection_line_margin = node.declare_parameter(ns + ".keep_detection_line_margin", 1.0); + ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index f8a66a06db665..fee2eb22b3fef 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -122,17 +122,19 @@ bool IntersectionModule::modifyPathVelocity( debug_data_.detection_area_with_margin = detection_areas_with_margin; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int pass_judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path, - &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs, + logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); return false; } + const int stop_line_idx = stop_line_idxs.stop_line_idx; + const int pass_judge_line_idx = stop_line_idxs.pass_judge_line_idx; + const int keep_detection_line_idx = stop_line_idxs.keep_detection_line_idx; if (stop_line_idx <= 0) { RCLCPP_DEBUG(logger_, "stop line line is at path[0], ignore planning."); RCLCPP_DEBUG(logger_, "===== plan end ====="); @@ -152,21 +154,32 @@ bool IntersectionModule::modifyPathVelocity( } const size_t closest_idx = closest_idx_opt.get(); - /* if current_state = GO, and current_pose is in front of stop_line, ignore planning. */ - bool is_over_pass_judge_line = - static_cast(static_cast(closest_idx) > pass_judge_line_idx); - if (static_cast(closest_idx) == pass_judge_line_idx) { - geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; - is_over_pass_judge_line = planning_utils::isAheadOf(current_pose.pose, pass_judge_line); - } - if (is_go_out_ && is_over_pass_judge_line && !external_stop) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx).point.pose.position)); - return true; // no plan needed. + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); + if (is_over_pass_judge_line) { + /* + in case ego could not stop exactly before the stop line, but with some overshoot, keep + detection within some margin and low velocity threshold + */ + const bool is_before_keep_detection_line = + util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); + if ( + is_before_keep_detection_line && std::fabs(planner_data_->current_velocity->twist.linear.x) < + planner_param_.keep_detection_vel_thr) { + RCLCPP_DEBUG( + logger_, + "over the pass judge line, but before keep detection line and low speed, " + "continue planning"); + // no return here, keep planning + } else if (is_go_out_ && !external_stop) { + RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx).point.pose.position)); + return true; // no plan needed. + } } /* get dynamic object */ @@ -201,22 +214,24 @@ bool IntersectionModule::modifyPathVelocity( if (!isActivated()) { constexpr double v = 0.0; is_go_out_ = false; + int stop_line_idx_stop = stop_line_idx; + int pass_judge_line_idx_stop = pass_judge_line_idx; if (planner_param_.use_stuck_stopline && is_stuck) { int stuck_stop_line_idx = -1; int stuck_pass_judge_line_idx = -1; if (util::generateStopLineBeforeIntersection( lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx, &stuck_pass_judge_line_idx, logger_.get_child("util"))) { - stop_line_idx = stuck_stop_line_idx; - pass_judge_line_idx = stuck_pass_judge_line_idx; + stop_line_idx_stop = stuck_stop_line_idx; + pass_judge_line_idx_stop = stuck_pass_judge_line_idx; } } - planning_utils::setVelocityFromIndex(stop_line_idx, v, path); + planning_utils::setVelocityFromIndex(stop_line_idx_stop, v, path); debug_data_.stop_required = true; debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, base_link2front, *path); - debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose; + planning_utils::getAheadPose(stop_line_idx_stop, base_link2front, *path); + debug_data_.stop_point_pose = path->points.at(stop_line_idx_stop).point.pose; + debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_stop).point.pose; /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index eb1de17673ae6..02f992743ed33 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -83,27 +83,27 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( debug_data_.detection_area = conflicting_areas; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, - private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + 0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - if (stop_line_idx <= 0 || judge_line_idx <= 0) { - RCLCPP_DEBUG(logger_, "stop line or judge line is at path[0], ignore planning."); + const int stop_line_idx = stop_line_idxs.stop_line_idx; + if (stop_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; } debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + const int first_idx_inside_lane = stop_line_idxs.first_idx_inside_lane; if (first_idx_inside_lane != -1) { debug_data_.first_collision_point = path->points.at(first_idx_inside_lane).point.pose.position; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 1d3e703d5ae39..c821d1044d72c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -73,7 +73,7 @@ bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, return false; } -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx) { @@ -116,9 +116,10 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -132,7 +133,8 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(stop_line_margin / interval); + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int keep_detection_line_margin_idx_dist = std::ceil(keep_detection_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); @@ -143,6 +145,10 @@ bool generateStopLine( return false; } + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; /* generate stop point */ // If a stop_line is defined in lanelet_map, use it. // else, generates a local stop_line with considering the lane conflicts. @@ -163,32 +169,45 @@ bool generateStopLine( const auto first_idx_inside_lane_opt = motion_utils::findNearestIndex(original_path->points, first_inside_point, 10.0, M_PI_4); if (first_idx_inside_lane_opt) { - *first_idx_inside_lane = first_idx_inside_lane_opt.get(); + first_idx_inside_lane = first_idx_inside_lane_opt.get(); } - if (*first_idx_inside_lane == 0) { + if (first_idx_inside_lane == 0) { RCLCPP_DEBUG( logger, "path[0] is already in the detection area. This happens if you have " "already crossed the stop line or are very far from the intersection. Ignore computation."); - *stop_line_idx = 0; - *pass_judge_line_idx = 0; + stop_line_idxs->first_idx_inside_lane = 0; + stop_line_idxs->pass_judge_line_idx = 0; + stop_line_idxs->stop_line_idx = 0; + stop_line_idxs->keep_detection_line_idx = 0; return true; } - stop_idx_ip = std::max(first_idx_ip_inside_lane - 1 - margin_idx_dist - base2front_idx_dist, 0); + stop_idx_ip = + std::max(first_idx_ip_inside_lane - 1 - stop_line_margin_idx_dist - base2front_idx_dist, 0); + } + + /* insert keep_detection_line */ + const int keep_detection_idx_ip = std::min( + stop_idx_ip + keep_detection_line_margin_idx_dist, static_cast(path_ip.points.size()) - 1); + const auto inserted_keep_detection_point = path_ip.points.at(keep_detection_idx_ip).point.pose; + if (!util::getDuplicatedPointIdx( + *original_path, inserted_keep_detection_point.position, &keep_detection_line_idx)) { + keep_detection_line_idx = util::insertPoint(inserted_keep_detection_point, original_path); } /* insert stop_point */ const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as stop_line_idx - if (!util::hasDuplicatedPoint(*original_path, inserted_stop_point.position, stop_line_idx)) { - *stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + if (!util::getDuplicatedPointIdx(*original_path, inserted_stop_point.position, &stop_line_idx)) { + stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + ++keep_detection_line_idx; // the index is incremented by judge stop line insertion } /* if another stop point exist before intersection stop_line, disable judge_line. */ bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { + for (int i = 0; i < stop_line_idx; ++i) { if (std::fabs(original_path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { has_prior_stopline = true; break; @@ -199,23 +218,29 @@ bool generateStopLine( const int pass_judge_idx_ip = std::min( static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { - *pass_judge_line_idx = *stop_line_idx; + pass_judge_line_idx = stop_line_idx; } else { const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( - *original_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { - *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); - ++(*stop_line_idx); // stop index is incremented by judge line insertion + if (!util::getDuplicatedPointIdx( + *original_path, inserted_pass_judge_point.position, &pass_judge_line_idx)) { + pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); + ++stop_line_idx; // stop index is incremented by judge line insertion + ++keep_detection_line_idx; // same. } } + stop_line_idxs->first_idx_inside_lane = first_idx_inside_lane; + stop_line_idxs->pass_judge_line_idx = pass_judge_line_idx; + stop_line_idxs->stop_line_idx = stop_line_idx; + stop_line_idxs->keep_detection_line_idx = keep_detection_line_idx; + RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %d, pass_judge_idx = %d, stop_idx_ip = " "%d, pass_judge_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); + stop_line_idx, pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); return true; } @@ -507,7 +532,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as *stuck_stop_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_stop_point.position, stuck_stop_line_idx)) { *stuck_stop_line_idx = util::insertPoint(inserted_stop_point, output_path); } @@ -531,7 +556,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, output_path); ++(*stuck_stop_line_idx); // stop index is incremented by judge line insertion @@ -556,5 +581,28 @@ geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) pose.position = p; return pose; } + +bool isOverTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(current_pose, target_pose); + } + return static_cast(closest_idx > target_idx); +} + +bool isBeforeTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(target_pose, current_pose); + } + return static_cast(target_idx > closest_idx); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt index 2995442ee1b67..64778b4b7ffbe 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/motion_velocity_smoother/CMakeLists.txt @@ -25,7 +25,6 @@ set(SMOOTHER_SRC src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp src/trajectory_utils.cpp - src/linear_interpolation.cpp src/resample.cpp ) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp deleted file mode 100644 index 10c0bca1ddddb..0000000000000 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2021 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. - -#ifndef MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ -#define MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ - -#include - -#include -#include -#include - -namespace motion_velocity_smoother -{ -namespace linear_interpolation -{ -boost::optional> interpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index); -} -} // namespace motion_velocity_smoother - -#endif // MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 6ea62a101e39b..dcc5079c9f9fb 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -125,7 +125,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node double extract_ahead_dist; // forward waypoints distance from current position [m] double extract_behind_dist; // backward waypoints distance from current position [m] double stop_dist_to_prohibit_engage; // prevent to move toward close stop point - double delta_yaw_threshold; // for closest index calculation + double ego_nearest_dist_threshold; // for ego's closest index calculation + double ego_nearest_yaw_threshold; // for ego's closest index calculation + resampling::ResampleParam post_resample_param; AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 } node_param_{}; @@ -225,9 +227,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_closest_merged_velocity_; // helper functions - boost::optional findNearestIndex( - const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const; - boost::optional findNearestIndexFromEgo(const TrajectoryPoints & points) const; + size_t findNearestIndexFromEgo(const TrajectoryPoints & points) const; bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index 76fffd5f95901..97e4c974f9ad7 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -45,13 +45,13 @@ struct ResampleParam boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v_current, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold, - const ResampleParam & param, const bool use_zoh_for_v = true); + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v = true); boost::optional resampleTrajectory( const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds, - const bool use_zoh_for_v = true); + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 32e97b5511827..0603c8305cee9 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -74,7 +74,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, - [[maybe_unused]] const double delta_yaw_threshold) const override; + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const override; boost::optional applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 2d595e9c1a1d6..554bdcca890ef 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -16,7 +16,6 @@ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 0cbf3a2cbd733..d4b65e695bb89 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -48,7 +48,8 @@ class JerkFilteredSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const override; + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const override; void setParam(const Param & param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index c822c2625c29a..de1d3cd4491c3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -46,7 +46,7 @@ class L2PseudoJerkSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const override; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; void setParam(const Param & smoother_param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 145336211228f..dadb314edbb58 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -46,7 +46,7 @@ class LinfPseudoJerkSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const override; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; void setParam(const Param & smoother_param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 0d401af8e5ebf..0ec6330394a56 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -58,7 +58,7 @@ class SmootherBase virtual boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const = 0; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; virtual boost::optional applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index 8c73329c1808b..a1fee003703e3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -38,7 +38,7 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Pose; TrajectoryPoint calcInterpolatedTrajectoryPoint( - const TrajectoryPoints & trajectory, const Pose & target_pose); + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx); TrajectoryPoints extractPathAroundIndex( const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, 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 d21c050caf9c5..8a1042fafa1e3 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -1,6 +1,7 @@ + @@ -16,6 +17,7 @@ + diff --git a/planning/motion_velocity_smoother/src/linear_interpolation.cpp b/planning/motion_velocity_smoother/src/linear_interpolation.cpp deleted file mode 100644 index d77134302a486..0000000000000 --- a/planning/motion_velocity_smoother/src/linear_interpolation.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "motion_velocity_smoother/linear_interpolation.hpp" - -#include - -namespace motion_velocity_smoother -{ -/* - * linear interpolation - */ -namespace linear_interpolation -{ -boost::optional> interpolate( - const std::vector & sample_x, const std::vector & sample_value, - const std::vector & query_x) -{ - std::vector query_value; - auto isIncrease = [](const std::vector & x) { - if (x.empty()) { - return false; - } - for (size_t i = 0; i < x.size() - 1; ++i) { - if (x.at(i) > x.at(i + 1)) { - return false; - } - } - return true; - }; - - if (sample_x.empty() || sample_value.empty() || query_x.empty()) { - printf( - "[interpolate] some vector size is zero: sample_x.size() = %lu, sample_value.size() = %lu, " - "query_x.size() = %lu\n", - sample_x.size(), sample_value.size(), query_x.size()); - return {}; - } - - // check if inputs are valid - if ( - !isIncrease(sample_x) || !isIncrease(query_x) || query_x.front() < sample_x.front() || - sample_x.back() < query_x.back() || sample_x.size() != sample_value.size()) { - std::cerr << "[isIncrease] bad index, return false" << std::endl; - const bool b1 = !isIncrease(sample_x); - const bool b2 = !isIncrease(query_x); - const bool b3 = query_x.front() < sample_x.front(); - const bool b4 = sample_x.back() < query_x.back(); - const bool b5 = sample_x.size() != sample_value.size(); - printf("%d, %d, %d, %d, %d\n", b1, b2, b3, b4, b5); - printf("sample_x = [%f, %f]\n", sample_x.front(), sample_x.back()); - printf("query_x = [%f, %f]\n", query_x.front(), query_x.back()); - printf( - "sample_x.size() = %lu, sample_value.size() = %lu\n", sample_x.size(), sample_value.size()); - return {}; - } - - // calculate linear interpolation - int i = 0; - for (const auto idx : query_x) { - if (sample_x.at(i) == idx) { - query_value.push_back(sample_value.at(i)); - continue; - } - while (sample_x.at(i) < idx) { - ++i; - } - if (i <= 0 || static_cast(sample_x.size()) - 1 < i) { - std::cerr << "? something wrong. skip this idx." << std::endl; - continue; - } - - const double dist_base_idx = sample_x.at(i) - sample_x.at(i - 1); - const double dist_to_forward = sample_x.at(i) - idx; - const double dist_to_backward = idx - sample_x.at(i - 1); - if (dist_to_forward < 0.0 || dist_to_backward < 0.0) { - std::cerr << "?? something wrong. skip this idx. sample_x.at(i - 1) = " << sample_x.at(i - 1) - << ", idx = " << idx << ", sample_x.at(i) = " << sample_x.at(i) << std::endl; - continue; - } - - const double value = - (dist_to_backward * sample_value.at(i) + dist_to_forward * sample_value.at(i - 1)) / - dist_base_idx; - query_value.push_back(value); - } - return query_value; -} -} // namespace linear_interpolation -} // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index bd1065b8b891f..ba0018e26ce77 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -148,7 +148,8 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter update_param("extract_ahead_dist", p.extract_ahead_dist); update_param("extract_behind_dist", p.extract_behind_dist); update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage); - update_param("delta_yaw_threshold", p.delta_yaw_threshold); + update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold); + update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold); } { @@ -245,7 +246,8 @@ void MotionVelocitySmootherNode::initCommonParam() p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0); p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0); p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5); - p.delta_yaw_threshold = declare_parameter("delta_yaw_threshold", M_PI / 3.0); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); p.post_resample_param.max_trajectory_length = declare_parameter("post_max_trajectory_length", 300.0); p.post_resample_param.min_trajectory_length = @@ -379,7 +381,8 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // Note that output velocity is resampled by linear interpolation auto output_resampled = resampling::resampleTrajectory( output, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose, - node_param_.delta_yaw_threshold, node_param_.post_resample_param, false); + node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold, + node_param_.post_resample_param, false); if (!output_resampled) { RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory"); return; @@ -432,16 +435,10 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length - const auto input_closest = findNearestIndexFromEgo(traj_input); - - if (!input_closest) { - RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, 5000, "Cannot find the closest point from input trajectory"); - return prev_output_; - } + const size_t input_closest = findNearestIndexFromEgo(traj_input); auto traj_extracted = trajectory_utils::extractPathAroundIndex( - traj_input, *input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); + traj_input, input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); if (traj_extracted.empty()) { RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); return prev_output_; @@ -458,12 +455,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( applyExternalVelocityLimit(traj_extracted); // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close - const auto traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); - - if (!traj_extracted_closest) { - RCLCPP_WARN(get_logger(), "Cannot find the closest point from extracted trajectory"); - return prev_output_; - } + const size_t traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); // Apply velocity to approach stop point applyStopApproachingVelocity(traj_extracted); @@ -476,7 +468,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } // Smoothing velocity - if (!smoothVelocity(traj_extracted, *traj_extracted_closest, output)) { + if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { return prev_output_; } @@ -500,17 +492,13 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x, - current_pose_ptr_->pose, node_param_.delta_yaw_threshold); + current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); if (!traj_resampled) { RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); return false; } - const auto traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled); - - if (!traj_resampled_closest) { - RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); - return false; - } + const size_t traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled); // Set 0[m/s] in the terminal point if (!traj_resampled->empty()) { @@ -523,7 +511,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Clip trajectory from closest point TrajectoryPoints clipped; clipped.insert( - clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + clipped.end(), traj_resampled->begin() + traj_resampled_closest, traj_resampled->end()); std::vector debug_trajectories; if (!smoother_->apply( @@ -536,7 +524,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( traj_smoothed.insert( traj_smoothed.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); + traj_resampled->begin() + traj_resampled_closest); // For the endpoint of the trajectory if (!traj_smoothed.empty()) { @@ -545,10 +533,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Max velocity filter for safety trajectory_utils::applyMaximumVelocityLimit( - *traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); + traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); // Insert behind velocity for output's consistency - insertBehindVelocity(*traj_resampled_closest, type, traj_smoothed); + insertBehindVelocity(traj_resampled_closest, type, traj_smoothed); RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); if (publish_debug_trajs_) { @@ -567,10 +555,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( for (auto & debug_trajectory : debug_trajectories) { debug_trajectory.insert( debug_trajectory.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); - for (size_t i = 0; i < *traj_resampled_closest; ++i) { + traj_resampled->begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { debug_trajectory.at(i).longitudinal_velocity_mps = - debug_trajectory.at(*traj_resampled_closest).longitudinal_velocity_mps; + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; } } } @@ -592,8 +580,27 @@ void MotionVelocitySmootherNode::insertBehindVelocity( output.at(i).longitudinal_velocity_mps = output.at(output_closest).longitudinal_velocity_mps; output.at(i).acceleration_mps2 = output.at(output_closest).acceleration_mps2; } else { + // TODO(planning/control team) deal with overlapped lanes with the same direction + const size_t seg_idx = [&]() { + // with distance and yaw thresholds + const auto opt_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + if (opt_nearest_seg_idx) { + return opt_nearest_seg_idx.get(); + } + + // with distance threshold + const auto opt_second_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); + if (opt_second_nearest_seg_idx) { + return opt_second_nearest_seg_idx.get(); + } + + return motion_utils::findNearestSegmentIndex(prev_output_, output.at(i).pose.position); + }(); const auto prev_output_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose); + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); // output should be always positive: TODO(Horibe) think better way output.at(i).longitudinal_velocity_mps = @@ -605,11 +612,7 @@ void MotionVelocitySmootherNode::insertBehindVelocity( void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const { - const auto closest_optional = findNearestIndexFromEgo(trajectory); - if (!closest_optional) { - return; - } - const auto closest = *closest_optional; + const size_t closest = findNearestIndexFromEgo(trajectory); // stop distance calculation const double stop_dist_lim{50.0}; @@ -645,8 +648,11 @@ MotionVelocitySmootherNode::calcInitialMotion( return std::make_pair(initial_motion, type); } - const auto prev_output_closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_traj, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto prev_output_closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + prev_traj, current_pose_ptr_->pose, current_seg_idx); // when velocity tracking deviation is large const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; @@ -715,7 +721,10 @@ void MotionVelocitySmootherNode::overwriteStopPoint( } // Get Closest Point from Output - const auto nearest_output_point_idx = findNearestIndex(output, input.at(*stop_idx).pose); + // TODO(planning/control team) deal with overlapped lanes with the same directions + const auto nearest_output_point_idx = motion_utils::findNearestIndex( + output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); // check over velocity bool is_stop_velocity_exceeded{false}; @@ -759,13 +768,10 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( 0, traj.size(), max_velocity_with_deceleration_, traj); - const auto closest_idx = findNearestIndexFromEgo(traj); - if (!closest_idx) { - return; - } + const size_t closest_idx = findNearestIndexFromEgo(traj); double dist = 0.0; - for (size_t idx = *closest_idx; idx < traj.size() - 1; ++idx) { + for (size_t idx = closest_idx; idx < traj.size() - 1; ++idx) { dist += tier4_autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); if (dist > external_velocity_limit_dist_) { trajectory_utils::applyMaximumVelocityLimit( @@ -822,8 +828,11 @@ void MotionVelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose); + trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose, current_seg_idx); Float32Stamped vel_data{}; vel_data.stamp = this->now(); @@ -833,8 +842,11 @@ void MotionVelocitySmootherNode::publishClosestVelocity( void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + trajectory, current_pose_ptr_->pose, current_seg_idx); auto publishFloat = [=](const double data, const auto pub) { Float32Stamped msg{}; @@ -871,8 +883,11 @@ void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final { prev_output_ = final_result; - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(final_result, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + final_result, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + final_result, current_pose_ptr_->pose, current_seg_idx); prev_closest_point_ = closest_point; } @@ -898,8 +913,11 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit double MotionVelocitySmootherNode::calcTravelDistance() const { - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_output_, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + prev_output_, current_pose_ptr_->pose, current_seg_idx); if (prev_closest_point_) { const double travel_dist = @@ -925,17 +943,11 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( return trajectory; } -boost::optional MotionVelocitySmootherNode::findNearestIndexFromEgo( - const TrajectoryPoints & points) const -{ - return findNearestIndex(points, current_pose_ptr_->pose); -} - -boost::optional MotionVelocitySmootherNode::findNearestIndex( - const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const +size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { - return motion_utils::findNearestIndex( - points, p, std::numeric_limits::max(), node_param_.delta_yaw_threshold); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); } bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index f04a564bbdeda..674234ab287e4 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -26,16 +26,15 @@ namespace resampling { boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v_current, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold, - const ResampleParam & param, const bool use_zoh_for_v) + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) { // Arc length from the initial point to the closest point - const auto negative_front_arclength_value = motion_utils::calcSignedArcLength( - input, current_pose, 0, std::numeric_limits::max(), delta_yaw_threshold); - if (!negative_front_arclength_value) { - return {}; - } - const auto front_arclength_value = std::fabs(*negative_front_arclength_value); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint(input, current_pose); @@ -145,8 +144,8 @@ boost::optional resampleTrajectory( boost::optional resampleTrajectory( const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds, - const bool use_zoh_for_v) + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) { // input arclength const double trajectory_length = motion_utils::calcArcLength(input); @@ -169,12 +168,12 @@ boost::optional resampleTrajectory( // Step1. Resample front trajectory // Arc length from the initial point to the closest point - const auto negative_front_arclength_value = motion_utils::calcSignedArcLength( - input, current_pose, 0, std::numeric_limits::max(), delta_yaw_threshold); - if (!negative_front_arclength_value) { - return {}; - } - const auto front_arclength_value = std::fabs(*negative_front_arclength_value); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, + static_cast(0)); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) { out_arclength.push_back(s); } diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index e32d37b403a15..a43f67b00b53d 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -234,7 +234,8 @@ bool AnalyticalJerkConstrainedSmoother::apply( boost::optional AnalyticalJerkConstrainedSmoother::resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, - [[maybe_unused]] const double delta_yaw_threshold) const + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const { TrajectoryPoints output; if (input.empty()) { 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 9a55dbe673f63..191f40cc5dde6 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 @@ -14,6 +14,8 @@ #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "interpolation/linear_interpolation.hpp" + #include #include @@ -232,28 +234,24 @@ bool calcStopVelocityWithConstantJerkAccLimit( dists.push_back(dist); } - const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, dists); - const auto acc_at_wp = linear_interpolation::interpolate(xs, as, dists); - const auto jerk_at_wp = linear_interpolation::interpolate(xs, js, dists); - if (!vel_at_wp || !acc_at_wp || !jerk_at_wp) { - RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Interpolation error"); - 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); // 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) - << ", j: " << jerk_at_wp->at(i) << std::endl; + ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) + << ", j: " << jerk_at_wp.at(i) << std::endl; } RCLCPP_DEBUG( rclcpp::get_logger("velocity_planning_utils"), "Interpolated = %s", ssi.str().c_str()); - for (size_t i = 0; i < vel_at_wp->size(); ++i) { - output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i); - output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i); + for (size_t i = 0; i < vel_at_wp.size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i); } - for (size_t i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) { + for (size_t i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) { output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; output_trajectory.at(i).acceleration_mps2 = 0.0; } diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 2fcfc1c77fd8a..d3b8d79e2f376 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -100,10 +100,11 @@ bool JerkFilteredSmoother::apply( debug_trajectories[2] = filtered; // Resample TrajectoryPoints for Optimization + // TODO(planning/control team) deal with overlapped lanes with the same direction const auto initial_traj_pose = filtered.front().pose; auto opt_resampled_trajectory = resampling::resampleTrajectory( filtered, v0, initial_traj_pose, std::numeric_limits::max(), - base_param_.resample_param); + std::numeric_limits::max(), base_param_.resample_param); if (!opt_resampled_trajectory) { RCLCPP_WARN(logger_, "Resample failed!"); @@ -471,10 +472,11 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( boost::optional JerkFilteredSmoother::resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, current_pose, delta_yaw_threshold, base_param_.resample_param, + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, smoother_param_.jerk_filter_ds); } diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index da3764e307cc7..6b2ae568f0052 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -223,10 +223,11 @@ bool L2PseudoJerkSmoother::apply( boost::optional L2PseudoJerkSmoother::resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const + const double nearest_dist_threshold, const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param); + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); } } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 8030a0ff5c1da..90e72f8b083d8 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -235,10 +235,11 @@ bool LinfPseudoJerkSmoother::apply( boost::optional LinfPseudoJerkSmoother::resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const + const double nearest_dist_threshold, const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param); + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); } } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 9feed52eb8ff8..2f84ff48b1669 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -16,7 +16,6 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_velocity_smoother/linear_interpolation.hpp" #include #include @@ -59,7 +58,7 @@ inline double integ_v(double v0, double a0, double j0, double t) inline double integ_a(double a0, double j0, double t) { return a0 + j0 * t; } TrajectoryPoint calcInterpolatedTrajectoryPoint( - const TrajectoryPoints & trajectory, const Pose & target_pose) + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx) { TrajectoryPoint traj_p{}; traj_p.pose = target_pose; @@ -76,17 +75,14 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( return traj_p; } - const size_t segment_idx = - motion_utils::findNearestSegmentIndex(trajectory, target_pose.position); - - auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose); - auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose); + auto v1 = getTransVector3(trajectory.at(seg_idx).pose, trajectory.at(seg_idx + 1).pose); + auto v2 = getTransVector3(trajectory.at(seg_idx).pose, target_pose); // calc internal proportion const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; { - const auto & seg_pt = trajectory.at(segment_idx); - const auto & next_pt = trajectory.at(segment_idx + 1); + const auto & seg_pt = trajectory.at(seg_idx); + const auto & next_pt = trajectory.at(seg_idx + 1); traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); @@ -499,21 +495,14 @@ boost::optional applyDecelFilterWithJerkConstraint( distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); }); const std::vector distance{distance_all.begin() + start_index, it_end}; - const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, distance); - const auto acc_at_wp = linear_interpolation::interpolate(xs, as, distance); - - if (!vel_at_wp || !acc_at_wp) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), - "interpolation error"); - return {}; - } + const auto vel_at_wp = interpolation::lerp(xs, vs, distance); + const auto acc_at_wp = interpolation::lerp(xs, as, distance); - for (unsigned int i = 0; i < vel_at_wp->size(); ++i) { - output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i); - output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i); + for (unsigned int i = 0; i < vel_at_wp.size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i); } - for (unsigned int i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) { + for (unsigned int i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) { output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; output_trajectory.at(i).acceleration_mps2 = 0.0; } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 9166bec372e55..8302736cb6149 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -102,7 +102,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // Get Closest Stop Obstacle const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj, planner_data.target_obstacles); - if (!closest_stop_obstacle && closest_stop_obstacle->collision_points.empty()) { + if (!closest_stop_obstacle || closest_stop_obstacle->collision_points.empty()) { // delete marker const auto markers = motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);