diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp
index 53a5777acabf1..f1516512c8993 100644
--- a/common/tier4_planning_rviz_plugin/src/path/display.cpp
+++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp
@@ -33,9 +33,8 @@ void AutowarePathWithLaneIdDisplay::preProcessMessageDetail()
VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo());
updateVehicleInfo();
} catch (const std::exception & e) {
- RCLCPP_WARN_THROTTLE(
- rviz_ros_node_.lock()->get_raw_node()->get_logger(),
- *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s",
+ RCLCPP_WARN_ONCE(
+ rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s",
e.what());
}
}
@@ -113,9 +112,8 @@ void AutowarePathDisplay::preProcessMessageDetail()
VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo());
updateVehicleInfo();
} catch (const std::exception & e) {
- RCLCPP_WARN_THROTTLE(
- rviz_ros_node_.lock()->get_raw_node()->get_logger(),
- *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s",
+ RCLCPP_WARN_ONCE(
+ rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s",
e.what());
}
}
@@ -131,9 +129,8 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail()
VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo());
updateVehicleInfo();
} catch (const std::exception & e) {
- RCLCPP_WARN_THROTTLE(
- rviz_ros_node_.lock()->get_raw_node()->get_logger(),
- *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s",
+ RCLCPP_WARN_ONCE(
+ rviz_ros_node_.lock()->get_raw_node()->get_logger(), "Failed to get vehicle_info: %s",
e.what());
}
}
diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
index 128ed0f9cd0ae..df99552ac4fdc 100644
--- a/launch/tier4_planning_launch/launch/planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -21,6 +21,7 @@
+
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index b37c177dd79e6..85cf24e926190 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -49,6 +49,8 @@ def launch_setup(context, *args, **kwargs):
avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f:
avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+ with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f:
+ dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f:
lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"]
with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f:
@@ -90,6 +92,7 @@ def launch_setup(context, *args, **kwargs):
side_shift_param,
avoidance_param,
avoidance_by_lc_param,
+ dynamic_avoidance_param,
lane_change_param,
goal_planner_param,
pull_out_param,
diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md
index b1cb925f5e49f..f5a15916f31a9 100644
--- a/perception/lidar_centerpoint/README.md
+++ b/perception/lidar_centerpoint/README.md
@@ -56,8 +56,8 @@ You can download the onnx format of trained models by clicking on the links belo
- Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx)
- Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx)
-`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs.
-`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs.
+`Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs.
+`Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs.
## Standalone inference and visualization
diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md
index 79275858ee0ee..a16cd5945801b 100644
--- a/perception/radar_tracks_msgs_converter/README.md
+++ b/perception/radar_tracks_msgs_converter/README.md
@@ -1,6 +1,6 @@
# radar_tracks_msgs_converter
-This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).
+This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).
- Calculation cost is O(n).
- n: The number of radar objects
@@ -13,7 +13,8 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe
- `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic
- `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic
- Output
- - `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): The topic converted to Autoware's message
+ - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message
+ - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message
### Parameters
diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp
index a45d7c8fc8c73..d5738c80dcca9 100644
--- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp
+++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp
@@ -18,6 +18,7 @@
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
#include "autoware_auto_perception_msgs/msg/shape.hpp"
#include "autoware_auto_perception_msgs/msg/tracked_object.hpp"
@@ -33,6 +34,9 @@
namespace radar_tracks_msgs_converter
{
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjectKinematics;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::Shape;
using autoware_auto_perception_msgs::msg::TrackedObject;
@@ -69,7 +73,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
// Publisher
- rclcpp::Publisher::SharedPtr pub_radar_{};
+ rclcpp::Publisher::SharedPtr pub_tracked_objects_{};
+ rclcpp::Publisher::SharedPtr pub_detected_objects_{};
// Timer
rclcpp::TimerBase::SharedPtr timer_{};
@@ -86,7 +91,9 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node
NodeParam node_param_{};
// Core
+ geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance();
TrackedObjects convertRadarTrackToTrackedObjects();
+ DetectedObjects convertRadarTrackToDetectedObjects();
uint8_t convertClassification(const uint16_t classification);
};
diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml
index ba4cc4762ea7d..c9f4a31354247 100644
--- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml
+++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml
@@ -1,14 +1,16 @@
-
+
+
-
+
+
diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
index cfe6ca3e4cea0..eb4a497e70495 100644
--- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
+++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
@@ -88,7 +88,8 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt
transform_listener_ = std::make_shared(this);
// Publisher
- pub_radar_ = create_publisher("~/output/radar_objects", 1);
+ pub_tracked_objects_ = create_publisher("~/output/radar_tracked_objects", 1);
+ pub_detected_objects_ = create_publisher("~/output/radar_detected_objects", 1);
// Timer
const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period();
@@ -152,11 +153,99 @@ void RadarTracksMsgsConverterNode::onTimer()
node_param_.new_frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01));
TrackedObjects tracked_objects = convertRadarTrackToTrackedObjects();
+ DetectedObjects detected_objects = convertRadarTrackToDetectedObjects();
if (!tracked_objects.objects.empty()) {
- pub_radar_->publish(tracked_objects);
+ pub_tracked_objects_->publish(tracked_objects);
+ pub_detected_objects_->publish(detected_objects);
}
}
+DetectedObjects RadarTracksMsgsConverterNode::convertRadarTrackToDetectedObjects()
+{
+ DetectedObjects detected_objects;
+ detected_objects.header = radar_data_->header;
+ detected_objects.header.frame_id = node_param_.new_frame_id;
+ using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+ using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX;
+
+ for (auto & radar_track : radar_data_->tracks) {
+ DetectedObject detected_object;
+
+ detected_object.existence_probability = 1.0;
+
+ detected_object.shape.type = Shape::BOUNDING_BOX;
+ detected_object.shape.dimensions = radar_track.size;
+
+ // kinematics
+ DetectedObjectKinematics kinematics;
+ kinematics.has_twist = true;
+ kinematics.has_twist_covariance = true;
+
+ // convert by tf
+ geometry_msgs::msg::PoseStamped radar_pose_stamped{};
+ radar_pose_stamped.pose.position = radar_track.position;
+ geometry_msgs::msg::PoseStamped transformed_pose_stamped{};
+ tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_);
+ kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose;
+
+ {
+ auto & pose_cov = kinematics.pose_with_covariance.covariance;
+ auto & radar_position_cov = radar_track.position_covariance;
+ pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X];
+ pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y];
+ pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z];
+ pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y];
+ pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y];
+ pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z];
+ pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z];
+ pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z];
+ pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z];
+ }
+
+ // convert by tf
+ geometry_msgs::msg::Vector3Stamped radar_velocity_stamped{};
+ radar_velocity_stamped.vector = radar_track.velocity;
+ geometry_msgs::msg::Vector3Stamped transformed_vector3_stamped{};
+ tf2::doTransform(radar_velocity_stamped, transformed_vector3_stamped, *transform_);
+ kinematics.twist_with_covariance.twist.linear = transformed_vector3_stamped.vector;
+
+ // twist compensation
+ if (node_param_.use_twist_compensation) {
+ if (odometry_data_) {
+ kinematics.twist_with_covariance.twist.linear.x += odometry_data_->twist.twist.linear.x;
+ kinematics.twist_with_covariance.twist.linear.y += odometry_data_->twist.twist.linear.y;
+ kinematics.twist_with_covariance.twist.linear.z += odometry_data_->twist.twist.linear.z;
+ } else {
+ RCLCPP_INFO(get_logger(), "Odometry data is not coming");
+ }
+ }
+
+ {
+ auto & twist_cov = kinematics.twist_with_covariance.covariance;
+ auto & radar_vel_cov = radar_track.velocity_covariance;
+ twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X];
+ twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y];
+ twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z];
+ twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y];
+ twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y];
+ twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z];
+ twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z];
+ twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z];
+ twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z];
+ }
+ detected_object.kinematics = kinematics;
+
+ // classification
+ ObjectClassification classification;
+ classification.probability = 1.0;
+ classification.label = convertClassification(radar_track.classification);
+ detected_object.classification.emplace_back(classification);
+
+ detected_objects.objects.emplace_back(detected_object);
+ }
+ return detected_objects;
+}
+
TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
{
TrackedObjects tracked_objects;
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
index 0c27e5fd835d8..78383f6146414 100644
--- a/planning/behavior_path_planner/CMakeLists.txt
+++ b/planning/behavior_path_planner/CMakeLists.txt
@@ -15,6 +15,7 @@ set(common_src
src/scene_module/scene_module_visitor.cpp
src/scene_module/avoidance/avoidance_module.cpp
src/scene_module/avoidance_by_lc/module.cpp
+ src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
src/scene_module/pull_out/pull_out_module.cpp
src/scene_module/goal_planner/goal_planner_module.cpp
src/scene_module/side_shift/side_shift_module.cpp
@@ -66,6 +67,7 @@ else()
src/planner_manager.cpp
src/scene_module/avoidance/manager.cpp
src/scene_module/avoidance_by_lc/manager.cpp
+ src/scene_module/dynamic_avoidance/manager.cpp
src/scene_module/pull_out/manager.cpp
src/scene_module/goal_planner/manager.cpp
src/scene_module/side_shift/manager.cpp
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
new file mode 100644
index 0000000000000..98e2b21b8cc22
--- /dev/null
+++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
@@ -0,0 +1,20 @@
+/**:
+ ros__parameters:
+ dynamic_avoidance:
+ # 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: 1.0 # [m/s]
+
+ drivable_area_generation:
+ lat_offset_from_obstacle: 0.8 # [m]
+ time_to_avoid: 5.0 # [s]
+ max_lat_offset_to_avoid: 1.0 # [m]
diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
index f5f1afe805d9b..30626a4df30c4 100644
--- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml
+++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml
@@ -65,3 +65,10 @@
enable_simultaneous_execution_as_candidate_module: false
priority: 3
max_module_size: 1
+
+ dynamic_avoidance:
+ enable_module: false
+ enable_simultaneous_execution_as_approved_module: true
+ enable_simultaneous_execution_as_candidate_module: true
+ priority: 7
+ max_module_size: 1
diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md
new file mode 100644
index 0000000000000..c4e458311b093
--- /dev/null
+++ b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md
@@ -0,0 +1,22 @@
+# Dynamic avoidance design
+
+## Purpose / Role
+
+This is a module designed for avoiding obstacles which are running.
+Static obstacles such as parked vehicles are dealt with by the avoidance module.
+
+This module is under development.
+In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area.
+Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory.
+
+### Parameters
+
+| Name | Unit | Type | Description | Default value |
+| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ |
+| target_object.car | [-] | bool | The flag whether to avoid cars or not | true |
+| target_object.truck | [-] | bool | The flag whether to avoid trucks or not | true |
+| ... | [-] | bool | ... | ... |
+| target_object.min_obstacle_vel | [m/s] | double | Minimum obstacle velocity to avoid | 1.0 |
+| drivable_area_generation.lat_offset_from_obstacle | [m] | double | Lateral offset to avoid from obstacles | 0.8 |
+| drivable_area_generation.time_to_avoid | [s] | double | Elapsed time for avoiding an obstacle | 5.0 |
+| drivable_area_generation.max_lat_offset_to_avoid | [m] | double | Maximum lateral offset to avoid | 0.5 |
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 7024a78ae274b..be7f4c69f5c5f 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
@@ -21,6 +21,7 @@
#ifdef USE_OLD_ARCHITECTURE
#include "behavior_path_planner/behavior_tree_manager.hpp"
#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
+#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
#include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
#include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
@@ -29,6 +30,7 @@
#include "behavior_path_planner/planner_manager.hpp"
#include "behavior_path_planner/scene_module/avoidance/manager.hpp"
#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp"
+#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp"
#include "behavior_path_planner/scene_module/goal_planner/manager.hpp"
#include "behavior_path_planner/scene_module/lane_change/manager.hpp"
#include "behavior_path_planner/scene_module/pull_out/manager.hpp"
@@ -74,6 +76,7 @@ namespace behavior_path_planner
{
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
@@ -148,6 +151,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
// parameters
std::shared_ptr avoidance_param_ptr_;
std::shared_ptr avoidance_by_lc_param_ptr_;
+ std::shared_ptr dynamic_avoidance_param_ptr_;
std::shared_ptr side_shift_param_ptr_;
std::shared_ptr lane_change_param_ptr_;
std::shared_ptr pull_out_param_ptr_;
@@ -160,6 +164,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
#endif
AvoidanceParameters getAvoidanceParam();
+ DynamicAvoidanceParameters getDynamicAvoidanceParam();
LaneChangeParameters getLaneChangeParam();
SideShiftParameters getSideShiftParam();
GoalPlannerParameters getGoalPlannerParam();
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
index a7ec6eb0df2ca..686a3233ade82 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -43,6 +43,7 @@
namespace behavior_path_planner
{
using autoware_adapi_v1_msgs::msg::OperationModeState;
+using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
index 6a9974fb482ef..2e310b2e9dfb7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
@@ -35,6 +35,7 @@ struct BehaviorPathPlannerParameters
ModuleConfigParameters config_avoidance;
ModuleConfigParameters config_avoidance_by_lc;
+ ModuleConfigParameters config_dynamic_avoidance;
ModuleConfigParameters config_pull_out;
ModuleConfigParameters config_goal_planner;
ModuleConfigParameters config_side_shift;
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
new file mode 100644
index 0000000000000..50a2d2ce5aa45
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -0,0 +1,116 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
+
+#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace behavior_path_planner
+{
+struct DynamicAvoidanceParameters
+{
+ // obstacle types to avoid
+ bool avoid_car{true};
+ bool avoid_truck{true};
+ bool avoid_bus{true};
+ bool avoid_trailer{true};
+ bool avoid_unknown{false};
+ bool avoid_bicycle{false};
+ bool avoid_motorcycle{false};
+ bool avoid_pedestrian{false};
+ double min_obstacle_vel{0.0};
+
+ // drivable area generation
+ double lat_offset_from_obstacle{0.0};
+ double time_to_avoid{0.0};
+ double max_lat_offset_to_avoid{0.0};
+};
+
+class DynamicAvoidanceModule : public SceneModuleInterface
+{
+public:
+ struct DynamicAvoidanceObject
+ {
+ explicit DynamicAvoidanceObject(
+ const PredictedObject & predicted_object, const double arg_path_projected_vel)
+ : pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
+ path_projected_vel(arg_path_projected_vel),
+ shape(predicted_object.shape)
+ {
+ }
+
+ geometry_msgs::msg::Pose pose;
+ double path_projected_vel;
+ autoware_auto_perception_msgs::msg::Shape shape;
+ };
+
+#ifdef USE_OLD_ARCHITECTURE
+ DynamicAvoidanceModule(
+ const std::string & name, rclcpp::Node & node,
+ std::shared_ptr parameters);
+#else
+ DynamicAvoidanceModule(
+ const std::string & name, rclcpp::Node & node,
+ std::shared_ptr parameters,
+ const std::unordered_map > & rtc_interface_ptr_map);
+
+ void updateModuleParams(const std::shared_ptr & parameters)
+ {
+ parameters_ = parameters;
+ }
+#endif
+
+ bool isExecutionRequested() const override;
+ bool isExecutionReady() const override;
+ ModuleStatus updateState() override;
+ ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; }
+ BehaviorModuleOutput plan() override;
+ CandidateOutput planCandidate() const override;
+ BehaviorModuleOutput planWaitingApproval() override;
+ void updateData() override;
+ void acceptVisitor(
+ [[maybe_unused]] const std::shared_ptr & visitor) const override
+ {
+ }
+
+private:
+ std::vector calcTargetObjects() const;
+ lanelet::ConstLanelets getAdjacentLanes(
+ const double forward_distance, const double backward_distance) const;
+ std::optional calcDynamicObstaclePolygon(
+ const PathWithLaneId & path, const DynamicAvoidanceObject & object) const;
+
+ std::vector target_objects_;
+ std::shared_ptr parameters_;
+};
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp
new file mode 100644
index 0000000000000..51fe30c8dc7ee
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp
@@ -0,0 +1,53 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_
+
+#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
+#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+
+#include
+
+#include
+#include
+#include
+#include
+
+namespace behavior_path_planner
+{
+
+class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface
+{
+public:
+ DynamicAvoidanceModuleManager(
+ rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+ const std::shared_ptr & parameters);
+
+ std::shared_ptr createNewSceneModuleInstance() override
+ {
+ return std::make_shared(
+ name_, *node_, parameters_, rtc_interface_ptr_map_);
+ }
+
+ void updateModuleParams(const std::vector & parameters) override;
+
+private:
+ std::shared_ptr parameters_;
+ std::vector> registered_modules_;
+};
+
+} // namespace behavior_path_planner
+
+#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
index 33ff4da7d77ff..43514b011c6fb 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
@@ -165,8 +165,8 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
const Pose & curent_pose, const double abort_return_dist,
const BehaviorPathPlannerParameters & common_param);
-lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck(
- const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lanes,
+lanelet::ConstLanelets getBackwardLanelets(
+ const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length);
LaneChangeTargetObjectIndices filterObjectIndices(
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 82c0c73f52b12..4f66e46c8d0b1 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -121,6 +121,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
// set parameters
{
avoidance_param_ptr_ = std::make_shared(getAvoidanceParam());
+ dynamic_avoidance_param_ptr_ =
+ std::make_shared(getDynamicAvoidanceParam());
lane_change_param_ptr_ = std::make_shared(getLaneChangeParam());
pull_out_param_ptr_ = std::make_shared(getPullOutParam());
goal_planner_param_ptr_ = std::make_shared(getGoalPlannerParam());
@@ -294,6 +296,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
"avoidance_by_lane_change",
create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1));
}
+
+ if (p.config_dynamic_avoidance.enable_module) {
+ auto manager = std::make_shared(
+ this, "dynamic_avoidance", p.config_dynamic_avoidance, dynamic_avoidance_param_ptr_);
+ planner_manager_->registerSceneModuleManager(manager);
+ }
}
#endif
@@ -349,6 +357,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
get_scene_module_manager_param("external_request_lane_change_left.");
p.config_avoidance = get_scene_module_manager_param("avoidance.");
p.config_avoidance_by_lc = get_scene_module_manager_param("avoidance_by_lc.");
+ p.config_dynamic_avoidance = get_scene_module_manager_param("dynamic_avoidance.");
// vehicle info
const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
@@ -647,6 +656,33 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
return p;
}
+DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
+{
+ DynamicAvoidanceParameters p{};
+
+ { // target object
+ std::string ns = "dynamic_avoidance.target_object.";
+ p.avoid_car = declare_parameter(ns + "car");
+ p.avoid_truck = declare_parameter(ns + "truck");
+ p.avoid_bus = declare_parameter(ns + "bus");
+ p.avoid_trailer = declare_parameter(ns + "trailer");
+ p.avoid_unknown = declare_parameter(ns + "unknown");
+ p.avoid_bicycle = declare_parameter(ns + "bicycle");
+ p.avoid_motorcycle = declare_parameter(ns + "motorcycle");
+ p.avoid_pedestrian = declare_parameter(ns + "pedestrian");
+ p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel");
+ }
+
+ { // drivable_area_generation
+ std::string ns = "dynamic_avoidance.drivable_area_generation.";
+ p.lat_offset_from_obstacle = declare_parameter(ns + "lat_offset_from_obstacle");
+ p.time_to_avoid = declare_parameter(ns + "time_to_avoid");
+ p.max_lat_offset_to_avoid = declare_parameter(ns + "max_lat_offset_to_avoid");
+ }
+
+ return p;
+}
+
LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
{
LaneChangeParameters p{};
diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
index 336c1c8d0a051..b4075932b80ab 100644
--- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
+++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp
@@ -88,6 +88,9 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
<< "move_time:" << object.move_time << " [s]\n"
<< "stop_time:" << object.stop_time << " [s]\n";
marker.text = string_stream.str();
+ marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999);
+ marker.scale = createMarkerScale(0.5, 0.5, 0.5);
+ marker.ns = ns;
msg.markers.push_back(marker);
}
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 8c06b074998c3..a8e09dae9e703 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
@@ -3328,7 +3328,6 @@ void AvoidanceModule::setDebugData(
add(createOverhangFurthestLineStringMarkerArray(
debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0));
- add(createUnavoidableObjectsMarkerArray(debug.unavoidable_objects, "unavoidable_objects"));
add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects"));
// parent object info
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
index f7103df9c1647..e3f032d3d0be3 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
@@ -948,15 +948,15 @@ bool AvoidanceByLCModule::isApprovedPathSafe(Pose & ego_pose_before_collision) c
const auto & path = status_.lane_change_path;
// get lanes used for detection
- const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck(
- *route_handler, path.target_lanelets.front(), current_pose, check_distance_);
+ const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets(
+ *route_handler, path.target_lanelets, current_pose, check_distance_);
std::unordered_map debug_data;
const auto lateral_buffer =
utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width);
const auto dynamic_object_indices = utils::lane_change::filterObjectIndices(
- {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length,
- *lane_change_parameters, lateral_buffer);
+ {path}, *dynamic_objects, backward_target_lanes_for_object_filtering, current_pose,
+ common_parameters.forward_path_length, *lane_change_parameters, lateral_buffer);
return utils::lane_change::isLaneChangePathSafe(
path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters,
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
new file mode 100644
index 0000000000000..d745b40d4bf39
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -0,0 +1,379 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
+
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace behavior_path_planner
+{
+namespace
+{
+bool isCentroidWithinLanelets(
+ const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets)
+{
+ if (target_lanelets.empty()) {
+ return false;
+ }
+
+ const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
+ lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y);
+
+ for (const auto & llt : target_lanelets) {
+ if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+std::vector getObjectsInLanes(
+ const std::vector & objects, const lanelet::ConstLanelets & target_lanes)
+{
+ std::vector target_objects;
+ for (const auto & object : objects) {
+ if (isCentroidWithinLanelets(object, target_lanes)) {
+ target_objects.push_back(object);
+ }
+ }
+
+ return target_objects;
+}
+
+geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point)
+{
+ geometry_msgs::msg::Point geom_obj_point;
+ geom_obj_point.x = point.x();
+ geom_obj_point.y = point.y();
+ return geom_obj_point;
+}
+
+double calcObstacleProjectedVelocity(
+ const std::vector & path_points, const PredictedObject & object)
+{
+ const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
+ const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
+
+ const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position);
+
+ const double obj_yaw = tf2::getYaw(obj_pose.orientation);
+ const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation);
+
+ return obj_vel * std::cos(obj_yaw - path_yaw);
+}
+
+std::pair getMinMaxValues(const std::vector & vec)
+{
+ const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end()));
+
+ const size_t max_idx = std::distance(vec.begin(), std::max_element(vec.begin(), vec.end()));
+
+ return std::make_pair(vec.at(min_idx), vec.at(max_idx));
+}
+} // namespace
+
+#ifdef USE_OLD_ARCHITECTURE
+DynamicAvoidanceModule::DynamicAvoidanceModule(
+ const std::string & name, rclcpp::Node & node,
+ std::shared_ptr parameters)
+: SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})},
+ parameters_{std::move(parameters)}
+#else
+DynamicAvoidanceModule::DynamicAvoidanceModule(
+ const std::string & name, rclcpp::Node & node,
+ std::shared_ptr parameters,
+ const std::unordered_map > & rtc_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)}
+#endif
+{
+}
+
+bool DynamicAvoidanceModule::isExecutionRequested() const
+{
+ RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested.");
+
+ const auto prev_module_path = getPreviousModuleOutput().path;
+ if (!prev_module_path || prev_module_path->points.size() < 2) {
+ return false;
+ }
+
+ // check if the ego is driving forward
+ const auto is_driving_forward = motion_utils::isDrivingForward(prev_module_path->points);
+ if (!is_driving_forward || !(*is_driving_forward)) {
+ return false;
+ }
+
+ // check if the planner is already running
+ if (current_state_ == ModuleStatus::RUNNING) {
+ return true;
+ }
+
+ // check if there is target objects to avoid
+ return !target_objects_.empty();
+}
+
+bool DynamicAvoidanceModule::isExecutionReady() const
+{
+ RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionReady.");
+ return true;
+}
+
+void DynamicAvoidanceModule::updateData()
+{
+ target_objects_ = calcTargetObjects();
+}
+
+ModuleStatus DynamicAvoidanceModule::updateState()
+{
+ const bool has_avoidance_target = !target_objects_.empty();
+
+ if (!has_avoidance_target) {
+ current_state_ = ModuleStatus::SUCCESS;
+ } else {
+ current_state_ = ModuleStatus::RUNNING;
+ }
+
+ return current_state_;
+}
+
+BehaviorModuleOutput DynamicAvoidanceModule::plan()
+{
+ // 1. get reference path from previous module
+ const auto prev_module_path = getPreviousModuleOutput().path;
+
+ // 2. get drivable lanes from previous module
+ const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes;
+
+ // 3. create obstacle polygons to avoid
+ std::vector obstacle_polys;
+ for (const auto & object : target_objects_) {
+ const auto obstacle_poly = calcDynamicObstaclePolygon(*prev_module_path, object);
+ if (obstacle_poly) {
+ obstacle_polys.push_back(obstacle_poly.value());
+ }
+ }
+
+ BehaviorModuleOutput output;
+ output.path = prev_module_path;
+ output.reference_path = getPreviousModuleOutput().reference_path;
+ // for new architecture
+ output.drivable_area_info.drivable_lanes = drivable_lanes;
+ output.drivable_area_info.obstacle_polys = obstacle_polys;
+ output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
+
+ return output;
+}
+
+CandidateOutput DynamicAvoidanceModule::planCandidate() const
+{
+ auto candidate_path = utils::generateCenterLinePath(planner_data_);
+ return CandidateOutput(*candidate_path);
+}
+
+BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval()
+{
+ BehaviorModuleOutput out = plan();
+ return out;
+}
+
+std::vector
+DynamicAvoidanceModule::calcTargetObjects() const
+{
+ const auto prev_module_path = getPreviousModuleOutput().path;
+
+ // 1. calculate target lanes to filter obstacles
+ const auto target_lanes = getAdjacentLanes(100.0, 10.0);
+
+ // 2. filter obstacles for dynamic avoidance
+ const auto & predicted_objects = planner_data_->dynamic_object->objects;
+ const auto target_predicted_objects = getObjectsInLanes(predicted_objects, target_lanes);
+
+ // 3. convert predicted objects to dynamic avoidance objects
+ std::vector target_avoidance_objects;
+ for (const auto & predicted_object : target_predicted_objects) {
+ const double path_projected_vel =
+ calcObstacleProjectedVelocity(prev_module_path->points, predicted_object);
+ // check if velocity is high enough
+ if (std::abs(path_projected_vel) < parameters_->min_obstacle_vel) {
+ continue;
+ }
+
+ target_avoidance_objects.push_back(
+ DynamicAvoidanceObject(predicted_object, path_projected_vel));
+ }
+
+ return target_avoidance_objects;
+}
+
+lanelet::ConstLanelets DynamicAvoidanceModule::getAdjacentLanes(
+ const double forward_distance, const double backward_distance) const
+{
+ const auto & rh = planner_data_->route_handler;
+
+ lanelet::ConstLanelet current_lane;
+ if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
+ "failed to find closest lanelet within route!!!");
+ return {};
+ }
+
+ const auto ego_succeeding_lanes =
+ rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance);
+
+ lanelet::ConstLanelets target_lanes;
+ for (const auto & lane : ego_succeeding_lanes) {
+ const auto opt_left_lane = rh->getLeftLanelet(lane);
+ if (opt_left_lane) {
+ target_lanes.push_back(opt_left_lane.get());
+ }
+
+ const auto opt_right_lane = rh->getRightLanelet(lane);
+ if (opt_right_lane) {
+ target_lanes.push_back(opt_right_lane.get());
+ }
+
+ const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane);
+ if (!right_opposite_lanes.empty()) {
+ target_lanes.push_back(right_opposite_lanes.front());
+ }
+ }
+
+ return target_lanes;
+}
+
+std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon(
+ const PathWithLaneId & path, const DynamicAvoidanceObject & object) const
+{
+ auto path_for_bound = path;
+
+ const size_t obj_seg_idx =
+ motion_utils::findNearestSegmentIndex(path_for_bound.points, object.pose.position);
+ const double obj_lat_offset =
+ motion_utils::calcLateralOffset(path_for_bound.points, object.pose.position, obj_seg_idx);
+ const bool is_left = 0.0 < obj_lat_offset;
+ const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
+
+ // calculate min/max lateral offset from object to path
+ const auto [min_obj_lat_abs_offset, max_obj_lat_abs_offset] = [&]() {
+ std::vector obj_lat_abs_offset_vec;
+ for (size_t i = 0; i < obj_points.outer().size(); ++i) {
+ const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i));
+ const size_t obj_point_seg_idx =
+ motion_utils::findNearestSegmentIndex(path_for_bound.points, geom_obj_point);
+ const double obj_point_lat_offset =
+ motion_utils::calcLateralOffset(path_for_bound.points, geom_obj_point, obj_point_seg_idx);
+ obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset));
+ }
+ return getMinMaxValues(obj_lat_abs_offset_vec);
+ }();
+ const double min_obj_lat_offset = min_obj_lat_abs_offset * (is_left ? 1.0 : -1.0);
+ const double max_obj_lat_offset = max_obj_lat_abs_offset * (is_left ? 1.0 : -1.0);
+
+ // calculate min/max longitudinal offset from object to path
+ const auto [min_obj_lon_offset, max_obj_lon_offset] = [&]() {
+ std::vector obj_lon_offset_vec;
+ for (size_t i = 0; i < obj_points.outer().size(); ++i) {
+ const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i));
+ const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment(
+ path_for_bound.points, obj_seg_idx, geom_obj_point);
+ obj_lon_offset_vec.push_back(lon_offset);
+ }
+ return getMinMaxValues(obj_lon_offset_vec);
+ }();
+
+ // calculate bound start and end index
+ const double length_to_avoid = object.path_projected_vel * parameters_->time_to_avoid;
+ [[maybe_unused]] const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
+ obj_seg_idx, min_obj_lon_offset + (length_to_avoid < 0 ? length_to_avoid : 0.0),
+ path_for_bound.points);
+ const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint(
+ obj_seg_idx, max_obj_lon_offset + (0 < length_to_avoid ? length_to_avoid : 0.0),
+ path_for_bound.points);
+ const size_t lon_bound_start_idx = obj_seg_idx;
+ const size_t lon_bound_end_idx = lon_bound_end_idx_opt
+ ? lon_bound_end_idx_opt.value()
+ : static_cast(path_for_bound.points.size() - 1);
+
+ // TODO(murooka) insertTargetPoint does not work well with a negative offset for now.
+ // When a offset is negative, the bound will be weird.
+ if (
+ lon_bound_start_idx == 0 &&
+ lon_bound_end_idx == static_cast(path_for_bound.points.size() - 1)) {
+ return std::nullopt;
+ }
+
+ // calculate bound min and max lateral offset
+ const double min_bound_lat_offset = [&]() {
+ const double raw_min_bound_lat_offset =
+ min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (is_left ? 1.0 : -1.0);
+ const double min_bound_lat_abs_offset_limit =
+ planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid;
+ if (is_left) {
+ if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) {
+ return min_bound_lat_abs_offset_limit;
+ }
+ } else {
+ if (-min_bound_lat_abs_offset_limit < raw_min_bound_lat_offset) {
+ return -min_bound_lat_abs_offset_limit;
+ }
+ }
+ return raw_min_bound_lat_offset;
+ }();
+ const double max_bound_lat_offset =
+ max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (is_left ? 1.0 : -1.0);
+
+ // 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_for_bound.points.at(i).point.pose, 0.0, min_bound_lat_offset, 0.0)
+ .position);
+ obj_outer_bound_points.push_back(
+ tier4_autoware_utils::calcOffsetPose(
+ path_for_bound.points.at(i).point.pose, 0.0, max_bound_lat_offset, 0.0)
+ .position);
+ }
+
+ // create obj_polygon from inner/outer bound points
+ tier4_autoware_utils::Polygon2d obj_poly;
+ for (const auto & bound_point : obj_inner_bound_points) {
+ const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y);
+ obj_poly.outer().push_back(obj_poly_point);
+ }
+ std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end());
+ for (const auto & bound_point : obj_outer_bound_points) {
+ const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y);
+ obj_poly.outer().push_back(obj_poly_point);
+ }
+
+ boost::geometry::correct(obj_poly);
+ return obj_poly;
+}
+} // namespace behavior_path_planner
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
new file mode 100644
index 0000000000000..3b7a74515f2dc
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp
@@ -0,0 +1,46 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp"
+
+#include
+
+#include
+#include
+#include
+
+namespace behavior_path_planner
+{
+
+DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
+ rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+ const std::shared_ptr & parameters)
+: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters}
+{
+}
+
+void DynamicAvoidanceModuleManager::updateModuleParams(
+ [[maybe_unused]] const std::vector & parameters)
+{
+ using tier4_autoware_utils::updateParam;
+
+ auto & p = parameters_;
+
+ [[maybe_unused]] std::string ns = name_ + ".";
+
+ std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) {
+ m->updateModuleParams(p);
+ });
+}
+} // namespace behavior_path_planner
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 c94d051b0551c..3ec87716f8035 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
@@ -492,11 +492,12 @@ bool NormalLaneChange::getLaneChangePaths(
if (candidate_paths->empty()) {
// only compute dynamic object indices once
- const auto backward_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck(
- route_handler, target_lanelets.front(), getEgoPose(), check_length_);
+ const auto backward_target_lanes_for_object_filtering =
+ utils::lane_change::getBackwardLanelets(
+ route_handler, target_lanelets, getEgoPose(), check_length_);
dynamic_object_indices = utils::lane_change::filterObjectIndices(
- {*candidate_path}, *dynamic_objects, backward_lanes, getEgoPose(),
- common_parameter.forward_path_length, *parameters_, lateral_buffer);
+ {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering,
+ getEgoPose(), common_parameter.forward_path_length, *parameters_, lateral_buffer);
}
candidate_paths->push_back(*candidate_path);
@@ -532,15 +533,15 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons
const auto & path = status_.lane_change_path;
// get lanes used for detection
- const auto check_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck(
- *route_handler, path.target_lanelets.front(), current_pose, check_length_);
+ const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets(
+ *route_handler, path.target_lanelets, current_pose, check_length_);
CollisionCheckDebugMap debug_data;
const auto lateral_buffer =
utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width);
const auto dynamic_object_indices = utils::lane_change::filterObjectIndices(
- {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length,
- lane_change_parameters, lateral_buffer);
+ {path}, *dynamic_objects, backward_target_lanes_for_object_filtering, current_pose,
+ common_parameters.forward_path_length, lane_change_parameters, lateral_buffer);
return utils::lane_change::isLaneChangePathSafe(
path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters,
diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
index 4a036738e8421..67cdd72511585 100644
--- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -402,10 +402,10 @@ bool getLaneChangePaths(
if (candidate_paths->empty()) {
// only compute dynamic object indices once
- const auto backward_lanes = utils::lane_change::getExtendedTargetLanesForCollisionCheck(
- route_handler, target_lanelets.front(), pose, check_length);
+ const auto backward_target_lanes_for_object_filtering =
+ utils::lane_change::getBackwardLanelets(route_handler, target_lanelets, pose, check_length);
dynamic_object_indices = filterObjectIndices(
- {*candidate_path}, *dynamic_objects, backward_lanes, pose,
+ {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, pose,
common_parameter.forward_path_length, parameter, lateral_buffer);
}
candidate_paths->push_back(*candidate_path);
@@ -497,7 +497,7 @@ bool isLaneChangePathSafe(
path.points, current_pose, common_parameter.ego_nearest_dist_threshold,
common_parameter.ego_nearest_yaw_threshold);
- const auto vehicle_predicted_path = utils::convertToPredictedPath(
+ const auto ego_predicted_path = utils::convertToPredictedPath(
path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution,
prepare_duration, acceleration);
const auto & vehicle_info = common_parameter.vehicle_info;
@@ -541,7 +541,7 @@ bool isLaneChangePathSafe(
for (double t = check_start_time; t < check_end_time; t += time_resolution) {
tier4_autoware_utils::Polygon2d ego_polygon;
const auto result =
- utils::getEgoExpectedPoseAndConvertToPolygon(vehicle_predicted_path, t, vehicle_info);
+ utils::getEgoExpectedPoseAndConvertToPolygon(ego_predicted_path, t, vehicle_info);
if (!result) {
continue;
}
@@ -552,9 +552,9 @@ bool isLaneChangePathSafe(
for (const auto & i : in_lane_object_indices) {
const auto & obj = dynamic_objects->objects.at(i);
auto current_debug_data = assignDebugData(obj);
- const auto predicted_paths =
+ const auto obj_predicted_paths =
utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path);
- for (const auto & obj_path : predicted_paths) {
+ for (const auto & obj_path : obj_predicted_paths) {
if (!utils::safety_check::isSafeInLaneletCollisionCheck(
interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare,
obj, obj_path, common_parameter,
@@ -574,7 +574,7 @@ bool isLaneChangePathSafe(
for (const auto & i : dynamic_objects_indices.other_lane) {
const auto & obj = dynamic_objects->objects.at(i);
auto current_debug_data = assignDebugData(obj);
- current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path);
+ current_debug_data.second.ego_predicted_path.push_back(ego_predicted_path);
const auto predicted_paths =
utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path);
@@ -1115,18 +1115,23 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
}
// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
-lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck(
- const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lane,
+lanelet::ConstLanelets getBackwardLanelets(
+ const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length)
{
- const auto arc_length = lanelet::utils::getArcCoordinates({target_lane}, current_pose);
+ if (target_lanes.empty()) {
+ return {};
+ }
+
+ const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
if (arc_length.length >= backward_length) {
return {};
}
+ const auto & front_lane = target_lanes.front();
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
- target_lane, std::abs(backward_length - arc_length.length), {target_lane});
+ front_lane, std::abs(backward_length - arc_length.length), {front_lane});
lanelet::ConstLanelets backward_lanes{};
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp
index 0bd83be6d059b..82230e93bb732 100644
--- a/planning/behavior_path_planner/src/utils/safety_check.cpp
+++ b/planning/behavior_path_planner/src/utils/safety_check.cpp
@@ -199,16 +199,15 @@ bool isSafeInLaneletCollisionCheck(
continue;
}
- const auto found_expected_obj_pose =
+ auto expected_obj_pose =
perception_utils::calcInterpolatedPose(target_object_path, current_time);
- if (!found_expected_obj_pose) {
+ if (!expected_obj_pose) {
continue;
}
- auto expected_obj_pose = *found_expected_obj_pose;
const auto & obj_polygon =
- tier4_autoware_utils::toPolygon2d(expected_obj_pose, target_object.shape);
+ tier4_autoware_utils::toPolygon2d(*expected_obj_pose, target_object.shape);
const auto & ego_info = interpolated_ego.at(i);
auto expected_ego_pose = ego_info.first;
const auto & ego_polygon = ego_info.second;
@@ -223,12 +222,12 @@ bool isSafeInLaneletCollisionCheck(
debug.lerped_path.push_back(expected_ego_pose);
getProjectedDistancePointFromPolygons(
- ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose);
+ ego_polygon, obj_polygon, expected_ego_pose, *expected_obj_pose);
debug.expected_ego_pose = expected_ego_pose;
- debug.expected_obj_pose = expected_obj_pose;
+ debug.expected_obj_pose = *expected_obj_pose;
if (!hasEnoughDistance(
- expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters,
+ expected_ego_pose, ego_current_twist, *expected_obj_pose, object_twist, common_parameters,
front_decel, rear_decel, debug)) {
debug.failed_reason = "not_enough_longitudinal";
return false;
diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
index eb1e9e6acede8..28eac75a17e0a 100644
--- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
+++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
@@ -52,18 +52,20 @@ std::shared_ptr generateNode()
"bt_tree_config_path", behavior_path_planner_dir + "/config/behavior_path_planner_tree.xml");
test_utils::updateNodeOptions(
- node_options, {planning_test_utils_dir + "/config/test_common.param.yaml",
- planning_test_utils_dir + "/config/test_nearest_search.param.yaml",
- planning_test_utils_dir + "/config/test_vehicle_info.param.yaml",
- behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml",
- behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml",
- behavior_path_planner_dir + "/config/scene_module_manager.param.yaml",
- behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml",
- behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml",
- behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml",
- behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml",
- behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml",
- behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"});
+ node_options,
+ {planning_test_utils_dir + "/config/test_common.param.yaml",
+ planning_test_utils_dir + "/config/test_nearest_search.param.yaml",
+ planning_test_utils_dir + "/config/test_vehicle_info.param.yaml",
+ behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml",
+ behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml",
+ behavior_path_planner_dir + "/config/scene_module_manager.param.yaml",
+ behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml",
+ behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml",
+ behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml",
+ behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml",
+ behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml",
+ behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml",
+ behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"});
return std::make_shared(node_options);
}
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 85b49217b0fdc..e64e26c1555d7 100644
--- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -189,17 +189,16 @@ void IntersectionModuleManager::sendRTC(const Time & stamp)
const bool is_occluded = intersection_module->isOccluded();
const UUID uuid = getUUID(scene_module->getModuleId());
const auto occlusion_uuid = intersection_module->getOcclusionUUID();
+ const auto occlusion_distance = intersection_module->getOcclusionDistance();
const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID();
if (!is_occluded) {
// default
updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp);
occlusion_rtc_interface_.updateCooperateStatus(
- occlusion_uuid, true, std::numeric_limits::lowest(),
- std::numeric_limits::lowest(), stamp);
+ occlusion_uuid, true, occlusion_distance, occlusion_distance, stamp);
} else {
// occlusion
const auto occlusion_safety = intersection_module->getOcclusionSafety();
- const auto occlusion_distance = intersection_module->getOcclusionDistance();
occlusion_rtc_interface_.updateCooperateStatus(
occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp);
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 059d0ef15d7e4..e32b6695a0cec 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
@@ -385,17 +385,17 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
logger_.get_child("collision state_machine"), *clock_);
/* set RTC safety respectively */
+ occlusion_first_stop_distance_ = dist_1st_stopline;
+ occlusion_stop_distance_ = dist_2nd_stopline;
+ setDistance(dist_1st_stopline);
if (occlusion_stop_required) {
if (first_phase_stop_required) {
occlusion_first_stop_safety_ = false;
- occlusion_first_stop_distance_ = dist_1st_stopline;
}
occlusion_safety_ = is_occlusion_cleared;
- occlusion_stop_distance_ = dist_2nd_stopline;
} else {
/* collision */
setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
- setDistance(dist_1st_stopline);
}
/* make decision */
diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp
index db5520a9142d5..d3d73927c2727 100644
--- a/planning/behavior_velocity_planner/src/utilization/util.cpp
+++ b/planning/behavior_velocity_planner/src/utilization/util.cpp
@@ -677,6 +677,7 @@ std::set getAssociativeIntersectionLanelets(
for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id());
}
std::set assocs;
+ assocs.insert(lane.id());
for (const auto & parent_neighbor_id : parent_neighbors) {
const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id);
const auto followings = routing_graph->following(parent_neighbor);