diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 8168ad2c9f265..fe95d21eb266c 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -41,12 +41,11 @@ class RoiDetectedObjectFusionNode : public FusionNode generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection); + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); void fuseObjectsOnImage( - const std::vector & objects, + const DetectedObjects & input_object_msg, const std::vector & image_rois, const std::map & object_roi_map); @@ -63,7 +62,8 @@ class RoiDetectedObjectFusionNode : public FusionNode passthrough_object_flags_, fused_object_flags_, ignored_object_flags_; + std::map> passthrough_object_flags_map_, fused_object_flags_map_, + ignored_object_flags_map_; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 37637f99f69c9..8961c0aad478f 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -34,19 +34,23 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { - passthrough_object_flags_.clear(); - passthrough_object_flags_.resize(output_msg.objects.size()); - fused_object_flags_.clear(); - fused_object_flags_.resize(output_msg.objects.size()); - ignored_object_flags_.clear(); - ignored_object_flags_.resize(output_msg.objects.size()); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; + passthrough_object_flags.resize(output_msg.objects.size()); + fused_object_flags.resize(output_msg.objects.size()); + ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { if ( output_msg.objects.at(obj_i).existence_probability > fusion_params_.passthrough_lower_bound_probability_threshold) { - passthrough_object_flags_.at(obj_i) = true; + passthrough_object_flags.at(obj_i) = true; } } + + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + passthrough_object_flags_map_.insert(std::make_pair(timestamp_nsec, passthrough_object_flags)); + fused_object_flags_map_.insert(std::make_pair(timestamp_nsec, fused_object_flags)); + ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags)); } void RoiDetectedObjectFusionNode::fuseOnSingleImage( @@ -58,8 +62,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } @@ -73,9 +77,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( camera_info.p.at(11); const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg.objects, static_cast(camera_info.width), + input_object_msg, static_cast(camera_info.width), static_cast(camera_info.height), object2camera_affine, camera_projection); - fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map); + fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); @@ -87,17 +91,25 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection) + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) { std::map object_roi_map; - for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (passthrough_object_flags_map_.size() == 0) { + return object_roi_map; + } + if (passthrough_object_flags_map_.count(timestamp_nsec) == 0) { + return object_roi_map; + } + const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; { - const auto & object = input_objects.at(obj_i); + const auto & object = input_object_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { continue; } @@ -142,7 +154,7 @@ std::map RoiDetectedObjectFusionNode::generateDet } } } - if (point_on_image_cnt == 0) { + if (point_on_image_cnt == 3) { continue; } @@ -168,13 +180,26 @@ std::map RoiDetectedObjectFusionNode::generateDet } void RoiDetectedObjectFusionNode::fuseObjectsOnImage( - const std::vector & objects __attribute__((unused)), + const DetectedObjects & input_object_msg, const std::vector & image_rois, const std::map & object_roi_map) { + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { + return; + } + if ( + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + for (const auto & object_pair : object_roi_map) { const auto & obj_i = object_pair.first; - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { continue; } @@ -192,15 +217,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( if (max_iou > fusion_params_.min_iou_threshold) { if (fusion_params_.use_roi_probability) { if (roi_prob > fusion_params_.roi_probability_threshold) { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } else { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } } @@ -234,19 +259,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) return; } + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + if ( + passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || + ignored_object_flags_map_.size() == 0) { + return; + } + if ( + passthrough_object_flags_map_.count(timestamp_nsec) == 0 || + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; output_objects_msg.header = output_msg.header; debug_fused_objects_msg.header = output_msg.header; debug_ignored_objects_msg.header = output_msg.header; for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); } - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); - } else if (ignored_object_flags_.at(obj_i)) { + } + if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } @@ -255,6 +298,10 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + + passthrough_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); } } // namespace image_projection_based_fusion diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index db2436f796dfe..22de79f4dc7d0 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,10 +11,10 @@ # avoidance module common setting enable_bound_clipping: false - enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false + enable_cancel_maneuver: false disable_path_update: false # drivable area setting diff --git a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml new file mode 100644 index 0000000000000..896c44c9cec9a --- /dev/null +++ b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + dynamic_avoidance: + common: + enable_debug_info: true + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: true + pedestrian: false + + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 + + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + + crossing_object: + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 + + front_object: + max_object_angle: 0.785 + + drivable_area_generation: + lat_offset_from_obstacle: 0.8 # [m] + max_lat_offset_to_avoid: 0.5 # [m] + + # for same directional object + overtaking_object: + max_time_to_collision: 40.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] + duration_to_hold_avoidance: 3.0 # [s] + + # for opposite directional object + oncoming_object: + max_time_to_collision: 15.0 # [s] + start_duration_to_avoid: 12.0 # [s] + end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 336879185f48e..701f05eb89ba1 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -3,6 +3,7 @@ dynamic_avoidance: common: enable_debug_info: true + use_hatched_road_markings: true # avoidance is performed for the object type with true target_object: @@ -18,6 +19,7 @@ min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 min_obj_lat_offset_to_ego_path: 0.0 # [m] max_obj_lat_offset_to_ego_path: 1.0 # [m] @@ -26,26 +28,34 @@ min_time_to_start_cut_in: 1.0 # [s] min_lon_offset_ego_to_object: 0.0 # [m] + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + crossing_object: - min_object_vel: 1.0 - max_object_angle: 1.05 + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 front_object: max_object_angle: 0.785 drivable_area_generation: - lat_offset_from_obstacle: 1.0 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] + max_time_for_object_lat_shift: 2.0 # [s] + lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] # for same directional object overtaking_object: - max_time_to_collision: 10.0 # [s] + max_time_to_collision: 40.0 # [s] start_duration_to_avoid: 2.0 # [s] end_duration_to_avoid: 4.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 15.0 # [s] + max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 9be99f705f560..4e4d2258fc276 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -601,13 +601,13 @@ $$ ### Avoidance cancelling maneuver -If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows: +If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows: - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. -If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. +If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. ## How to keep the consistency of the optimize-base path generation logic @@ -621,15 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow namespace: `avoidance.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | +| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | +| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | +| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | +| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e6ed8b7063d2c..54cfccc494e8a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; @@ -102,6 +104,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; + rclcpp::Publisher::SharedPtr reroute_availability_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; + /** + * @brief publish reroute availability + */ + void publish_reroute_availability(); + /** * @brief publish steering factor from intersection */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 02e5452cc2103..f1292c2db7811 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -214,6 +214,16 @@ class PlannerManager return stop_reason_array; } + /** + * @brief check if there are approved modules. + */ + bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } + + /** + * @brief check if there are candidate modules. + */ + bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); } + /** * @brief reset root lanelet. if there are approved modules, don't reset root lanelet. * @param planner data. 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 b6cc840a560f0..3095ec2c03251 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 @@ -195,19 +195,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - /** - * @brief check whether the ego is shifted based on shift line. - * @return result. - */ - bool isAvoidanceManeuverRunning(); - - /** - * @brief check whether the ego is in avoidance maneuver based on shift line and target object - * existence. - * @return result. - */ - bool isAvoidancePlanRunning() const; - // ego behavior update /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index c93707bf84362..1f54786df83d7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -45,6 +45,7 @@ struct DynamicAvoidanceParameters { // common bool enable_debug_info{true}; + bool use_hatched_road_markings{true}; // obstacle types to avoid bool avoid_car{true}; @@ -67,12 +68,16 @@ struct DynamicAvoidanceParameters double max_time_from_outside_ego_path_for_cut_out{0.0}; double min_cut_out_object_lat_vel{0.0}; double max_front_object_angle{0.0}; - double min_crossing_object_vel{0.0}; - double max_crossing_object_angle{0.0}; + double min_overtaking_crossing_object_vel{0.0}; + double max_overtaking_crossing_object_angle{0.0}; + double min_oncoming_crossing_object_vel{0.0}; + double max_oncoming_crossing_object_angle{0.0}; // drivable area generation double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; + double max_time_for_lat_shift{0.0}; + double lpf_gain_for_lat_avoid_to_offset{0.0}; double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; @@ -115,8 +120,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; - MinMaxValue lon_offset_to_avoid; - MinMaxValue lat_offset_to_avoid; + // NOTE: Previous values of the following are used for low-pass filtering. + // Therefore, they has to be initialized as nullopt. + std::optional lon_offset_to_avoid{std::nullopt}; + std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left; bool should_be_avoided{false}; @@ -235,6 +242,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::unordered_map object_map_; }; + struct DecisionWithReason + { + bool decision; + std::string reason{""}; + }; + DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, @@ -295,18 +308,17 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); - std::optional> calcPathForObjectPolygon() const; bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; - bool willObjectCutOut( + DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; double calcTimeToCollision( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( @@ -326,6 +338,14 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; + void printIgnoreReason(const std::string & obj_uuid, const std::string & reason) + { + const auto reason_text = + "[DynamicAvoidance] Ignore obstacle (%s)" + (reason == "" ? "." : " since " + reason + "."); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, reason_text.c_str(), obj_uuid.c_str()); + } + std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 2b1a9cf1b82d4..0263eee5fe727 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -92,8 +92,8 @@ struct AvoidanceParameters // to use this, enable_avoidance_over_same_direction need to be set to true. bool use_opposite_lane{true}; - // enable update path when if detected objects on planner data is gone. - bool enable_update_path_when_object_is_gone{false}; + // if this param is true, it reverts avoidance path when the path is no longer needed. + bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 869de6cb4d103..3e2ccd23d58e7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -68,6 +68,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); + reroute_availability_publisher_ = + create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); debug_lane_change_msg_array_publisher_ = @@ -539,6 +541,9 @@ void BehaviorPathPlannerNode::run() // compute turn signal computeTurnSignal(planner_data_, *path, output); + // publish reroute availability + publish_reroute_availability(); + // publish drivable bounds publish_bounds(*path); @@ -654,6 +659,26 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_reroute_availability() +{ + const bool has_approved_modules = planner_manager_->hasApprovedModules(); + const bool has_candidate_modules = planner_manager_->hasCandidateModules(); + + // In the current behavior path planner, we might get unexpected behavior when rerouting while + // modules other than lane follow are active. Therefore, rerouting will be allowed only when the + // lane follow module is running Note that if there is a approved module or candidate module, it + // means non-lane-following modules are runnning. + RerouteAvailability is_reroute_available; + is_reroute_available.stamp = this->now(); + if (has_approved_modules || has_candidate_modules) { + is_reroute_available.availability = false; + } else { + is_reroute_available.availability = true; + } + + reroute_availability_publisher_->publish(is_reroute_available); +} + void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data) { MarkerArray marker_array; 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 601badee6a9f9..db80bc72f21ad 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 @@ -135,64 +135,53 @@ bool AvoidanceModule::isExecutionReady() const bool AvoidanceModule::canTransitSuccessState() { const auto & data = avoidance_data_; - const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit."); return true; } - const auto idx = planner_data_->findEgoIndex(data.reference_path.points); - if (idx == data.reference_path.points.size() - 1) { - arrived_path_end_ = true; - } - - constexpr double THRESHOLD = 1.0; - if ( - calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD && - arrived_path_end_) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module."); - return true; - } + helper_.setPreviousDrivingLanes(data.current_lanelets); - DEBUG_PRINT( - "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + // Reach input path end point. -> EXIT. + { + const auto idx = planner_data_->findEgoIndex(data.reference_path.points); + if (idx == data.reference_path.points.size() - 1) { + arrived_path_end_ = true; + } - if (!is_plan_running && !has_avoidance_target) { - return true; + constexpr double THRESHOLD = 1.0; + const auto is_further_than_threshold = + calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD; + if (is_further_than_threshold && arrived_path_end_) { + RCLCPP_WARN(getLogger(), "Reach path end point. Exit."); + return true; + } } - if ( - !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && - !isAvoidanceManeuverRunning()) { - // if dynamic objects are removed on path, change current state to reset path - return true; + const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_shift_point = !path_shifter_.getShiftLines().empty(); + const bool has_base_offset = + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + + // Nothing to do. -> EXIT. + if (!has_avoidance_target) { + if (!has_shift_point && !has_base_offset) { + RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); + return true; + } } - helper_.setPreviousDrivingLanes(data.current_lanelets); - - return false; -} - -bool AvoidanceModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > AVOIDING_SHIFT_THR; - const bool has_shift_point = (path_shifter_.getShiftLinesSize() > 0); - return has_base_offset || has_shift_point; -} -bool AvoidanceModule::isAvoidanceManeuverRunning() -{ - const auto path_idx = avoidance_data_.ego_closest_path_index; - - for (const auto & al : registered_raw_shift_lines_) { - if (path_idx > al.start_idx || is_avoidance_maneuver_starts) { - is_avoidance_maneuver_starts = true; + // Be able to canceling avoidance path. -> EXIT. + if (!has_avoidance_target) { + if (!helper_.isShifted() && parameters_->enable_cancel_maneuver) { + RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); return true; } } - return false; + + return false; // Keep current state. } AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 13b3344dc7c39..a0ae98276c05e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -55,8 +55,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 52f887e60d83e..7f69397cb3cf5 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -68,7 +68,7 @@ void appendExtractedPolygonMarker( auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. @@ -270,7 +270,6 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() debug_marker_.markers.clear(); const auto prev_module_path = getPreviousModuleOutput().path; - const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; @@ -285,11 +284,18 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() } } + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = + getPreviousModuleOutput().drivable_area_info.drivable_lanes; + current_drivable_area_info.obstacles = obstacles_for_drivable_area; + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; + BehaviorModuleOutput output; output.path = prev_module_path; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; - output.drivable_area_info.drivable_lanes = drivable_lanes; - output.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; return output; @@ -343,11 +349,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto path_points_for_object_polygon = calcPathForObjectPolygon(); - if (!path_points_for_object_polygon) { - return; - } - + const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; const auto prev_objects = target_objects_manager_.getValidObjects(); // 1. Rough filtering of target objects @@ -374,11 +376,16 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.c. check if object is not crossing ego's path const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); - const bool is_obstacle_crossing_path = - parameters_->max_crossing_object_angle < std::abs(obj_angle) && - parameters_->max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double max_crossing_object_angle = 0.0 <= obj_tangent_vel + ? parameters_->max_overtaking_crossing_object_angle + : parameters_->max_oncoming_crossing_object_angle; + const bool is_obstacle_crossing_path = max_crossing_object_angle < std::abs(obj_angle) && + max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double min_crossing_object_vel = 0.0 <= obj_tangent_vel + ? parameters_->min_overtaking_crossing_object_vel + : parameters_->min_oncoming_crossing_object_vel; const bool is_crossing_object_to_ignore = - parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; if (is_crossing_object_to_ignore) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -459,34 +466,32 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.d. check if object will not cut out - const bool will_object_cut_out = + const auto will_object_cut_out = willObjectCutOut(object.vel, object.lat_vel, is_object_left, prev_object); - if (will_object_cut_out) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since it will cut out.", obj_uuid.c_str()); + if (will_object_cut_out.decision) { + printIgnoreReason(obj_uuid.c_str(), will_object_cut_out.reason); continue; } - // 2.e. check if time to collision + // 2.e. check time to collision const double time_to_collision = - calcTimeToCollision(prev_module_path->points, object.pose, object.vel); + calcTimeToCollision(prev_module_path->points, object.pose, object.vel, lat_lon_offset); if ( (0 <= object.vel && parameters_->max_time_to_collision_overtaking_object < time_to_collision) || (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision is large.", - obj_uuid.c_str()); + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", + obj_uuid.c_str(), time_to_collision); continue; } if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision is a small negative " + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative " "value.", - obj_uuid.c_str()); + obj_uuid.c_str(), time_to_collision); continue; } @@ -500,9 +505,9 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2.g. calculate longitudinal and lateral offset to avoid const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - *path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); + path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - *path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); + path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); const bool should_be_avoided = true; target_objects_manager_.updateObject( @@ -510,30 +515,6 @@ void DynamicAvoidanceModule::updateTargetObjects() } } -std::optional> DynamicAvoidanceModule::calcPathForObjectPolygon() - const -{ - const auto & ego_pose = getEgoPose(); - const auto & rh = planner_data_->route_handler; - - // get path with backward margin - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), - "failed to find closest lanelet within route!!!"); - return std::nullopt; - } - - constexpr double forward_length = 100.0; - const double backward_length = 50.0; - const auto current_lanes = - rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); - const auto path = utils::getCenterLinePath( - *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); - return path.points; -} - [[maybe_unused]] std::optional> DynamicAvoidanceModule::calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const @@ -574,15 +555,24 @@ DynamicAvoidanceModule::calcCollisionSection( double DynamicAvoidanceModule::calcTimeToCollision( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { - const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. + // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const double lon_offset_ego_to_obj = + motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + + lat_lon_offset.max_lon_offset; + const double maximum_time_to_collision = + (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits::max(); + + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); const double signed_lon_length = motion_utils::calcSignedArcLength( ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return signed_lon_length / positive_relative_velocity; + return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); } bool DynamicAvoidanceModule::isObjectFarFromPath( @@ -630,33 +620,45 @@ bool DynamicAvoidanceModule::willObjectCutIn( return false; } -bool DynamicAvoidanceModule::willObjectCutOut( +DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const { // Ignore oncoming object if (obj_tangent_vel < 0) { - return false; + return DecisionWithReason{false}; } - if (prev_object && prev_object->latest_time_inside_ego_path) { - if ( - parameters_->max_time_from_outside_ego_path_for_cut_out < - (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) { - return false; - } + // Check if previous object is memorized + if (!prev_object || !prev_object->latest_time_inside_ego_path) { + return DecisionWithReason{false}; + } + if ( + parameters_->max_time_from_outside_ego_path_for_cut_out < + (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) { + return DecisionWithReason{false}; } + // Check object's lateral velocity + std::stringstream reason; + reason << "since latest time inside ego's path is small enough (" + << (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds() << "<" + << parameters_->max_time_from_outside_ego_path_for_cut_out << ")"; if (is_object_left) { if (parameters_->min_cut_out_object_lat_vel < obj_normal_vel) { - return true; + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << "min_cut_out_object_lat_vel) { - return true; + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << " DynamicAvoidanceModule::getAdjacentLanes( @@ -803,10 +805,9 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( // calculate bound min and max lateral offset const double min_bound_lat_offset = [&]() { - constexpr double object_time_to_shift = 2.0; const double lat_abs_offset_to_shift = std::max(0.0, obj_normal_vel * (is_collision_left ? -1.0 : 1.0)) * - object_time_to_shift; // TODO(murooka) use rosparam + parameters_->max_time_for_lat_shift; const double raw_min_bound_lat_offset = min_obj_lat_abs_offset - parameters_->lat_offset_from_obstacle - lat_abs_offset_to_shift; const double min_bound_lat_abs_offset_limit = @@ -820,16 +821,16 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( // filter min_bound_lat_offset const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { - if (!prev_object) { + if (!prev_object || !prev_object->lat_offset_to_avoid) { return std::nullopt; } - return prev_object->lat_offset_to_avoid.min_value; + return prev_object->lat_offset_to_avoid->min_value; }(); const double filtered_min_bound_lat_offset = - prev_min_lat_avoid_to_offset - ? signal_processing::lowpassFilter( - min_bound_lat_offset, *prev_min_lat_avoid_to_offset, 0.5) // TODO(murooka) use rosparam - : min_bound_lat_offset; + prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( + min_bound_lat_offset, *prev_min_lat_avoid_to_offset, + parameters_->lpf_gain_for_lat_avoid_to_offset) + : min_bound_lat_offset; return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset}; } @@ -838,22 +839,23 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { - auto path_points_for_object_polygon = calcPathForObjectPolygon(); - if (!path_points_for_object_polygon) { + if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { return std::nullopt; } + auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(*path_points_for_object_polygon, object.pose.position); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid.min_value, *path_points_for_object_polygon); + obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid.max_value, *path_points_for_object_polygon); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -863,19 +865,19 @@ std::optional DynamicAvoidanceModule::calcDynam lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(path_points_for_object_polygon->size() - 1); + : static_cast(path_points_for_object_polygon.size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon->at(i).point.pose, 0.0, - object.lat_offset_to_avoid.min_value, 0.0) + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->min_value, 0.0) .position); obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon->at(i).point.pose, 0.0, - object.lat_offset_to_avoid.max_value, 0.0) + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->max_value, 0.0) .position); } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 87525068c8c34..e378d43f6497c 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -32,6 +32,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // common std::string ns = "dynamic_avoidance.common."; p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); + p.use_hatched_road_markings = node->declare_parameter(ns + "use_hatched_road_markings"); } { // target object @@ -68,16 +69,24 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.max_front_object_angle = node->declare_parameter(ns + "front_object.max_object_angle"); - p.min_crossing_object_vel = - node->declare_parameter(ns + "crossing_object.min_object_vel"); - p.max_crossing_object_angle = - node->declare_parameter(ns + "crossing_object.max_object_angle"); + p.min_overtaking_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_overtaking_object_vel"); + p.max_overtaking_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_overtaking_object_angle"); + p.min_oncoming_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel"); + p.max_oncoming_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle"); } { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); + p.max_time_for_lat_shift = + node->declare_parameter(ns + "max_time_for_object_lat_shift"); + p.lpf_gain_for_lat_avoid_to_offset = + node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); @@ -108,6 +117,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // common const std::string ns = "dynamic_avoidance.common."; updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); + updateParam(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); } { // target object @@ -152,9 +162,17 @@ void DynamicAvoidanceModuleManager::updateModuleParams( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); updateParam( - parameters, ns + "crossing_object.min_object_vel", p->min_crossing_object_vel); + parameters, ns + "crossing_object.min_overtaking_object_vel", + p->min_overtaking_crossing_object_vel); updateParam( - parameters, ns + "crossing_object.max_object_angle", p->max_crossing_object_angle); + parameters, ns + "crossing_object.max_overtaking_object_angle", + p->max_overtaking_crossing_object_angle); + updateParam( + parameters, ns + "crossing_object.min_oncoming_object_vel", + p->min_oncoming_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_oncoming_object_angle", + p->max_oncoming_crossing_object_angle); } { // drivable_area_generation @@ -162,6 +180,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + updateParam( + parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift); + updateParam( + parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); updateParam( parameters, ns + "overtaking_object.max_time_to_collision", diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 7f8272e748233..b8219c5057f1d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1076,11 +1076,22 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + constexpr double stuck_time = 5.0; + if (!isStopped(odometry_buffer_stuck_, stuck_time)) { + return false; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + // any path has never been found if (!status_.pull_over_path) { return false; } - constexpr double stuck_time = 5.0; - return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); + + return checkCollision(getCurrentPath()); } bool GoalPlannerModule::hasFinishedCurrentPath() @@ -1425,7 +1436,10 @@ void GoalPlannerModule::setDebugData() const auto header = planner_data_->route_handler->getRouteHeader(); - const auto add = [this](const MarkerArray & added) { + const auto add = [this](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; @@ -1467,7 +1481,8 @@ void GoalPlannerModule::setDebugData() auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_->goal_pose; + marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose + : planner_data_->self_odometry->pose.pose; marker.text = magic_enum::enum_name(status_.pull_over_path->type); marker.text += " " + std::to_string(status_.current_path_idx) + "/" + std::to_string(status_.pull_over_path->partial_paths.size() - 1); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 7c12579bf34f5..0f770efb542c7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -197,8 +197,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "detection_area_right_expand_dist"); p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); } 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 3d2bb44ad8bc0..2f7c39f94604e 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 @@ -540,6 +540,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( current_velocity, common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + // if maximum lane change length is less than length to goal or the end of target lanes, only // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 7aa0fd2d690fc..32398c7944435 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -79,7 +79,7 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getStartPose(); + const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 79e15bf4e68d7..7bff7bef578ce 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -137,8 +137,9 @@ multipolygon_t createExpansionPolygons( : footprint_dist; auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); + const auto uncrossable_dist_limit = std::max( + 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - + params.avoid_linestring_dist); if (uncrossable_dist_limit < limited_dist) { limited_dist = uncrossable_dist_limit; if (params.compensate_uncrossable_lines) { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index fd3b0d76107cb..391b397dd82cd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -252,6 +252,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Set safe or unsafe setSafe(!nearest_stop_factor); + // Set distance + // NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated. + setDistanceToStop(*path, default_stop_pose, nearest_stop_factor); + // plan Go/Stop if (isActivated()) { planGo(*path, nearest_stop_factor); @@ -402,7 +406,7 @@ std::pair CrosswalkModule::getAttentionRange( void CrosswalkModule::insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output) + PathWithLaneId & output) const { const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity); if (!stop_pose) { @@ -1007,25 +1011,34 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( return polygon; } -void CrosswalkModule::planGo( - PathWithLaneId & ego_path, const std::optional & stop_factor) +void CrosswalkModule::setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor) { - if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); - return; + // calculate stop position + const auto stop_pos = [&]() -> std::optional { + if (stop_factor) return stop_factor->stop_pose.position; + if (default_stop_pose) return default_stop_pose->position; + return std::nullopt; + }(); + + // Set distance + if (stop_pos) { + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); + setDistance(dist_ego2stop); } +} +void CrosswalkModule::planGo( + PathWithLaneId & ego_path, const std::optional & stop_factor) const +{ // Plan slow down const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path); insertDecelPointWithDebugInfo( stop_factor->stop_pose.position, std::max(planner_param_.min_slow_down_velocity, target_velocity), ego_path); - - // Set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } void CrosswalkModule::planStop( @@ -1038,22 +1051,11 @@ void CrosswalkModule::planStop( return std::nullopt; }(); - if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); - return; - } - // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); planning_utils::appendStopReason(*stop_factor, stop_reason); velocity_factor_.set( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); - - // set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 7ac00ab880be8..961126bf4bca1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -295,7 +295,12 @@ class CrosswalkModule : public SceneModuleInterface const std::optional & stop_factor_for_crosswalk_users, const std::optional & stop_factor_for_stuck_vehicles); - void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor); + void setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor); + + void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor) const; void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, @@ -308,7 +313,7 @@ class CrosswalkModule : public SceneModuleInterface void insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output); + PathWithLaneId & output) const; std::pair clampAttentionRangeByNeighborCrosswalks( const PathWithLaneId & ego_path, const double near_attention_range, diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f5f7eefd49d7d..49e1d0de8be93 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_planning_msgs vehicle_info_util ament_lint_auto diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..08da1260b000c 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,6 +75,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + odometry_(nullptr), + map_ptr_(nullptr), + reroute_availability_(nullptr), normal_route_(nullptr), mrm_route_(nullptr) { @@ -85,15 +88,19 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - odometry_ = nullptr; sub_odometry_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); + sub_reroute_availability_ = create_subscription( + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/" + "is_reroute_available", + rclcpp::QoS(1), + std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - vector_map_subscriber_ = create_subscription( + sub_vector_map_ = create_subscription( "input/vector_map", qos_transient_local, - std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); @@ -130,7 +137,12 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } -void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg) +{ + reroute_availability_ = msg; +} + +void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; } @@ -375,6 +387,10 @@ void MissionPlanner::on_set_mrm_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } const auto prev_state = state_.state; change_state(RouteState::Message::CHANGING); @@ -444,6 +460,10 @@ void MissionPlanner::on_clear_mrm_route( if (!mrm_route_) { throw component_interface_utils::NoEffectWarning("MRM route is not set"); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); @@ -574,6 +594,10 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -633,6 +657,10 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..fd1b317970749 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -55,6 +56,7 @@ using NormalRoute = planning_interface::NormalRoute; using MrmRoute = planning_interface::MrmRoute; using RouteState = planning_interface::RouteState; using Odometry = nav_msgs::msg::Odometry; +using RerouteAvailability = tier4_planning_msgs::msg::RerouteAvailability; class MissionPlanner : public rclcpp::Node { @@ -71,9 +73,14 @@ class MissionPlanner : public rclcpp::Node PoseStamped transform_pose(const PoseStamped & input); rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_reroute_availability_; Odometry::ConstSharedPtr odometry_; + HADMapBin::ConstSharedPtr map_ptr_; + RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_map(const HADMapBin::ConstSharedPtr msg); + void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -121,9 +128,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route(