diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp index fdfd3a25eb3ad..077c66f9dd6bc 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -40,10 +39,10 @@ class VelocityFactorInterface void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } + template void set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail = ""); + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail = ""); private: VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index eabd00fae85d6..20742af0b6c0c 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -15,12 +15,16 @@ #include #include +#include +#include +#include + namespace motion_utils { +template void VelocityFactorInterface::set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail) + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail) { const auto & curr_point = curr_pose.position; const auto & stop_point = stop_pose.position; @@ -32,4 +36,14 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); + } // namespace motion_utils diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 905b66df288b4..71eb92a4e95fb 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -301,10 +301,13 @@ bool AEB::fetchLatestData() } const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_ && !imu_ptr) { - return missing("imu message"); + if (use_imu_path_) { + if (!imu_ptr) { + return missing("imu message"); + } + // imu_ptr is valid + onImu(imu_ptr); } - onImu(imu_ptr); if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 18de04fd9e317..f8deccb188b71 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,6 +2,24 @@ + + + + + + + + + + @@ -91,12 +109,44 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml index dfdee95642fed..99050d9738ae6 100644 --- a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -9,3 +9,13 @@ MOTORCYCLE : false BICYCLE : false PEDESTRIAN : false + + filter_settings: + # polygon overlap based filter + polygon_overlap_filter: + enabled: true + # velocity direction based filter + lanelet_direction_filter: + enabled: false + velocity_yaw_threshold: 0.785398 # [rad] (45 deg) + object_speed_threshold: 3.0 # [m/s] diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index f3871aaf98117..00826de68157f 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,6 +17,7 @@ #include "detected_object_validation/utils/utils.hpp" +#include #include #include #include @@ -62,11 +63,27 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; - + struct FilterSettings + { + bool polygon_overlap_filter; + bool lanelet_direction_filter; + double lanelet_direction_filter_velocity_yaw_threshold; + double lanelet_direction_filter_object_speed_threshold; + } filter_settings_; + + bool filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + bool isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 50a81e95d5a9b..fe304a3ea22bb 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + // Set filter settings + filter_settings_.polygon_overlap_filter = + declare_parameter("filter_settings.polygon_overlap_filter.enabled"); + filter_settings_.lanelet_direction_filter = + declare_parameter("filter_settings.lanelet_direction_filter.enabled"); + filter_settings_.lanelet_direction_filter_velocity_yaw_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.velocity_yaw_threshold"); + filter_settings_.lanelet_direction_filter_object_speed_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.object_speed_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -97,27 +106,13 @@ void ObjectLaneletFilterNode::objectCallback( lanelet::ConstLanelets intersected_shoulder_lanelets = getIntersectedLanelets(convex_hull, shoulder_lanelets_); - int index = 0; - for (const auto & object : transformed_objects.objects) { - const auto footprint = setFootprint(object); - const auto & label = object.classification.front().label; - if (filter_target_.isTarget(label)) { - Polygon2d polygon; - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - } else { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - ++index; + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject( + transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, + output_object_msg); } object_pub_->publish(output_object_msg); published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); @@ -132,6 +127,55 @@ void ObjectLaneletFilterNode::objectCallback( "debug/pipeline_latency_ms", pipeline_latency); } +bool ObjectLaneletFilterNode::filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg) +{ + const auto & label = transformed_object.classification.front().label; + if (filter_target_.isTarget(label)) { + bool filter_pass = true; + // 1. is polygon overlap with road lanelets or shoulder lanelets + if (filter_settings_.polygon_overlap_filter) { + Polygon2d polygon; + const auto footprint = setFootprint(transformed_object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint( + point, transformed_object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + const bool is_polygon_overlap = + isPolygonOverlapLanelets(polygon, intersected_road_lanelets) || + isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets); + filter_pass = filter_pass && is_polygon_overlap; + } + + // 2. check if objects velocity is the same with the lanelet direction + const bool orientation_not_available = + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { + const bool is_same_direction = + isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || + isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + filter_pass = filter_pass && is_same_direction; + } + + // push back to output object + if (filter_pass) { + output_object_msg.objects.emplace_back(input_object); + return true; + } + } else { + output_object_msg.objects.emplace_back(input_object); + return true; + } + return false; +} + geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) { @@ -201,6 +245,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( return false; } +bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_velocity_norm = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double object_velocity_yaw = std::atan2( + object.kinematics.twist_with_covariance.twist.linear.y, + object.kinematics.twist_with_covariance.twist.linear.x) + + object_yaw; + + if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { + return true; + } + for (const auto & lanelet : lanelets) { + const bool is_in_lanelet = + lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + if (!is_in_lanelet) { + continue; + } + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_velocity_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { + return true; + } + } + + return false; +} + } // namespace object_lanelet_filter #include diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 353f797b0bee1..8a7f4c6107c67 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -159,6 +159,50 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(test_nms test/test_nms.cpp ) + ament_auto_add_gtest(test_voxel_generator + test/test_voxel_generator.cpp + ) + + add_executable(test_preprocess_kernel + test/test_preprocess_kernel.cpp + lib/utils.cpp + ) + + target_include_directories(test_preprocess_kernel PUBLIC + ${test_preprocess_kernel_SOURCE_DIR} + ) + + target_link_libraries(test_preprocess_kernel + centerpoint_cuda_lib + gtest + gtest_main + ) + + ament_add_test(test_preprocess_kernel + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + ) + + add_executable(test_postprocess_kernel + test/test_postprocess_kernel.cpp + lib/utils.cpp + ) + + target_include_directories(test_postprocess_kernel PUBLIC + ${test_postprocess_kernel_SOURCE_DIR} + ) + + target_link_libraries(test_postprocess_kernel + centerpoint_cuda_lib + gtest + gtest_main + ) + + ament_add_test(test_postprocess_kernel + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + ) + endif() else() diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 118e31f892d72..218aaee125c02 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -78,6 +78,11 @@ cudaError_t generateVoxels_random_launch( { dim3 blocks((points_size + 256 - 1) / 256); dim3 threads(256); + + if (blocks.x == 0) { + return cudaGetLastError(); + } + generateVoxels_random_kernel<<>>( points, points_size, min_x_range, max_x_range, min_y_range, max_y_range, min_z_range, max_z_range, pillar_x_size, pillar_y_size, pillar_z_size, grid_y_size, grid_x_size, mask, diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp new file mode 100644 index 0000000000000..fde4fcbee3d6c --- /dev/null +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -0,0 +1,367 @@ +// Copyright 2024 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 "test_postprocess_kernel.hpp" + +#include + +#include +#include +#include + +namespace centerpoint +{ + +void PostprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + constexpr std::size_t class_size{5}; + constexpr std::size_t point_feature_size{4}; + constexpr std::size_t max_voxel_size{100000000}; + const std::vector point_cloud_range{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; + const std::vector voxel_size{0.32, 0.32, 10.0}; + constexpr std::size_t downsample_factor{1}; + constexpr std::size_t encoder_in_feature_size{9}; + constexpr float score_threshold{0.35f}; + constexpr float circle_nms_dist_threshold{0.5f}; + const std::vector yaw_norm_thresholds{0.5, 0.5, 0.5}; + constexpr bool has_variance{false}; + + config_ptr_ = std::make_unique( + class_size, point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_thresholds, has_variance); + + postprocess_cuda_ptr_ = std::make_unique(*config_ptr_); + + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + + head_out_heatmap_d_ = cuda::make_unique(grid_xy_size * config_ptr_->class_size_); + head_out_offset_d_ = + cuda::make_unique(grid_xy_size * config_ptr_->head_out_offset_size_); + head_out_z_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_z_size_); + head_out_dim_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_dim_size_); + head_out_rot_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_rot_size_); + head_out_vel_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_vel_size_); + + std::vector heatmap_host_vector(grid_xy_size * config_ptr_->class_size_, 0.f); + std::fill(heatmap_host_vector.begin(), heatmap_host_vector.end(), -1e6); + cudaMemcpy( + head_out_heatmap_d_.get(), heatmap_host_vector.data(), + grid_xy_size * config_ptr_->head_out_offset_size_ * sizeof(float), cudaMemcpyHostToDevice); + cudaMemset( + head_out_offset_d_.get(), 0, grid_xy_size * config_ptr_->head_out_offset_size_ * sizeof(float)); + cudaMemset(head_out_z_d_.get(), 0, grid_xy_size * config_ptr_->head_out_z_size_ * sizeof(float)); + cudaMemset( + head_out_dim_d_.get(), 0, grid_xy_size * config_ptr_->head_out_dim_size_ * sizeof(float)); + cudaMemset( + head_out_rot_d_.get(), 0, grid_xy_size * config_ptr_->head_out_rot_size_ * sizeof(float)); + cudaMemset( + head_out_vel_d_.get(), 0, grid_xy_size * config_ptr_->head_out_vel_size_ * sizeof(float)); +} + +void PostprocessKernelTest::TearDown() +{ +} + +TEST_F(PostprocessKernelTest, EmptyTensorTest) +{ + std::vector det_boxes3d; + + postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, SingleDetectionTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_log_w = std::log(7.0); + constexpr float detection_log_l = std::log(1.0); + constexpr float detection_log_h = std::log(2.0); + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::sin(detection_yaw); + constexpr float detection_vel_x = 5.0; + constexpr float detection_vel_y = -5.0; + + const float idx = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index = config_ptr_->down_grid_size_x_ * std::floor(idy) + std::floor(idx); + const float detection_x_offset = idx - std::floor(idx); + const float detection_y_offset = idy - std::floor(idy); + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index], &detection_x_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index], &detection_y_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index], &detection_yaw_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(1, det_boxes3d.size()); + + const auto det_box3d = det_boxes3d[0]; + EXPECT_EQ(detection_x, det_box3d.x); + EXPECT_EQ(detection_y, det_box3d.y); + EXPECT_EQ(detection_z, det_box3d.z); + EXPECT_NEAR(std::exp(detection_log_w), det_box3d.width, 1e-6); + EXPECT_NEAR(std::exp(detection_log_l), det_box3d.length, 1e-6); + EXPECT_NEAR(std::exp(detection_log_h), det_box3d.height, 1e-6); + EXPECT_EQ(detection_yaw, det_box3d.yaw); + EXPECT_EQ(detection_vel_x, det_box3d.vel_x); + EXPECT_EQ(detection_vel_y, det_box3d.vel_y); +} + +TEST_F(PostprocessKernelTest, InvalidYawTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_yaw_sin = 0.0; + constexpr float detection_yaw_cos = 0.2; + + const float idx = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index = config_ptr_->down_grid_size_x_ * std::floor(idy) + std::floor(idx); + const float detection_x_offset = idx - std::floor(idx); + const float detection_y_offset = idy - std::floor(idy); + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index], &detection_x_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index], &detection_y_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index], &detection_yaw_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, CircleNMSTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_log_w = std::log(7.0); + constexpr float detection_log_l = std::log(1.0); + constexpr float detection_log_h = std::log(2.0); + constexpr float detection_yaw1_sin = 0.0; + constexpr float detection_yaw1_cos = 1.0; + constexpr float detection_yaw2_sin = 1.0; + constexpr float detection_yaw2_cos = 0.0; + constexpr float detection_vel_x = 5.0; + constexpr float detection_vel_y = -5.0; + + const float idx1 = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy1 = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index1 = config_ptr_->down_grid_size_x_ * std::floor(idy1) + std::floor(idx1); + const float detection_x_offset1 = idx1 - std::floor(idx1); + const float detection_y_offset1 = idy1 - std::floor(idy1); + + const float idx2 = idx1 + 1.0; + const float idy2 = idy1 + 1.0; + const std::size_t index2 = config_ptr_->down_grid_size_x_ * std::floor(idy2) + std::floor(idx2); + const float detection_x_offset2 = detection_x_offset1 - 1.0; + const float detection_y_offset2 = detection_y_offset1 - 1.0; + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index1], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index1], &detection_x_offset1, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index1], &detection_y_offset1, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index1], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index1], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index1], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index1], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index1], &detection_yaw1_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index1], &detection_yaw1_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index1], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index1], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index2], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index2], &detection_x_offset2, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index2], &detection_y_offset2, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index2], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index2], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index2], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index2], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index2], &detection_yaw2_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index2], &detection_yaw2_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index2], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index2], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(1, det_boxes3d.size()); +} + +} // namespace centerpoint + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp new file mode 100644 index 0000000000000..d0c9123da9e77 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 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 TEST_POSTPROCESS_KERNEL_HPP_ +#define TEST_POSTPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace centerpoint +{ + +class PostprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + cudaStream_t stream_{nullptr}; + + std::unique_ptr postprocess_cuda_ptr_; + std::unique_ptr config_ptr_; + + cuda::unique_ptr head_out_heatmap_d_; + cuda::unique_ptr head_out_offset_d_; + cuda::unique_ptr head_out_z_d_; + cuda::unique_ptr head_out_dim_d_; + cuda::unique_ptr head_out_rot_d_; + cuda::unique_ptr head_out_vel_d_; +}; + +} // namespace centerpoint + +#endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp new file mode 100644 index 0000000000000..db75328f9d5c5 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp @@ -0,0 +1,403 @@ +// Copyright 2024 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 "test_preprocess_kernel.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +namespace centerpoint +{ + +void PreprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + max_voxel_size_ = 40000; + max_point_in_voxel_size_ = + 32; // this value is actually hardcoded in the kernel so it can not be changed + point_feature_size_ = 4; + point_dim_size_ = 3; + config_encoder_in_feature_size_ = 9; + encoder_out_feature_size_ = 32; + capacity_ = 1000000; + + range_min_x_ = -76.8f; + range_min_y_ = -76.8f; + range_min_z_ = -4.0f; + range_max_x_ = 76.8f; + range_max_y_ = 76.8f; + range_max_z_ = 6.0f; + voxel_size_x_ = 0.32f; + voxel_size_y_ = 0.32f; + voxel_size_z_ = 10.0f; + + grid_size_x_ = static_cast((range_max_x_ - range_min_x_) / voxel_size_x_); + grid_size_y_ = static_cast((range_max_y_ - range_min_y_) / voxel_size_y_); + grid_size_z_ = static_cast((range_max_z_ - range_min_z_) / voxel_size_z_); + + voxels_size_ = max_voxel_size_ * max_point_in_voxel_size_ * point_feature_size_; + coordinates_size_ = max_voxel_size_ * point_dim_size_; + encoder_in_feature_size_ = + max_voxel_size_ * max_point_in_voxel_size_ * config_encoder_in_feature_size_; + pillar_features_size_ = max_voxel_size_ * encoder_out_feature_size_; + spatial_features_size_ = grid_size_x_ * grid_size_y_ * encoder_out_feature_size_; + grid_xy_size_ = grid_size_x_ * grid_size_y_; + + voxels_buffer_size_ = + grid_size_x_ * grid_size_y_ * max_point_in_voxel_size_ * point_feature_size_; + mask_size_ = grid_size_x_ * grid_size_y_; + + voxels_d_ = cuda::make_unique(voxels_size_); + coordinates_d_ = cuda::make_unique(coordinates_size_); + num_points_per_voxel_d_ = cuda::make_unique(max_voxel_size_); + encoder_in_features_d_ = cuda::make_unique(encoder_in_feature_size_); + pillar_features_d_ = cuda::make_unique(pillar_features_size_); + spatial_features_d_ = cuda::make_unique(spatial_features_size_); + points_d_ = cuda::make_unique(capacity_ * point_feature_size_); + voxels_buffer_d_ = cuda::make_unique(voxels_buffer_size_); + mask_d_ = cuda::make_unique(mask_size_); + num_voxels_d_ = cuda::make_unique(1); + + CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(mask_d_.get(), 0, mask_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(voxels_d_.get(), 0, voxels_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(coordinates_d_.get(), 0, coordinates_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(num_points_per_voxel_d_.get(), 0, max_voxel_size_ * sizeof(float), stream_)); +} + +void PreprocessKernelTest::TearDown() +{ +} + +TEST_F(PreprocessKernelTest, EmptyVoxelTest) +{ + std::vector points{}; + std::size_t points_num = 0; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(0, sum); +} + +TEST_F(PreprocessKernelTest, BasicTest) +{ + std::vector points{25.f, -61.1f, 1.8f, 0.1f}; + std::size_t points_num = 1; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(1, sum); + + // Check that the voxel was filled + int voxel_idx = std::floor((points[0] - range_min_x_) / voxel_size_x_); + int voxel_idy = std::floor((points[1] - range_min_y_) / voxel_size_y_); + unsigned int voxel_index = voxel_idy * grid_size_x_ + voxel_idx; + + unsigned voxel_count; + std::array voxel_data{0.f, 0.f, 0.f, 0.f}; + + CHECK_CUDA_ERROR(cudaMemcpy( + &voxel_count, mask_d_.get() + voxel_index, 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, voxel_count); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_data.data(), + voxels_buffer_d_.get() + voxel_index * max_point_in_voxel_size_ * point_feature_size_, + point_feature_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(points[0], voxel_data[0]); + EXPECT_EQ(points[1], voxel_data[1]); + EXPECT_EQ(points[2], voxel_data[2]); + EXPECT_EQ(points[3], voxel_data[3]); + + code = generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), grid_size_y_, grid_size_x_, max_voxel_size_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_); + + ASSERT_EQ(cudaSuccess, code); + + unsigned int num_pillars{}; + std::vector voxel_features(point_feature_size_, 0.f); + float num_voxels{}; + std::vector voxel_coordinates(3, 0); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_pillars, num_voxels_d_.get(), 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxels_d_.get(), point_feature_size_ * sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_voxels, num_points_per_voxel_d_.get(), 1 * sizeof(float), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_coordinates.data(), coordinates_d_.get(), 3 * sizeof(int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, num_pillars); + EXPECT_EQ(1.0, num_voxels); + EXPECT_EQ(0, voxel_coordinates[0]); + EXPECT_EQ(voxel_idy, voxel_coordinates[1]); + EXPECT_EQ(voxel_idx, voxel_coordinates[2]); + EXPECT_EQ(points[0], voxel_features[0]); + EXPECT_EQ(points[1], voxel_features[1]); + EXPECT_EQ(points[2], voxel_features[2]); + EXPECT_EQ(points[3], voxel_features[3]); + + code = generateFeatures_launch( + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), + max_voxel_size_, voxel_size_x_, voxel_size_y_, voxel_size_z_, range_min_x_, range_min_y_, + range_min_z_, encoder_in_features_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + std::vector encoder_features(config_encoder_in_feature_size_, 0.f); + + CHECK_CUDA_ERROR(cudaMemcpy( + encoder_features.data(), encoder_in_features_d_.get(), + config_encoder_in_feature_size_ * sizeof(float), cudaMemcpyDeviceToHost)); + + // The first four values are just the point features + EXPECT_EQ(points[0], encoder_features[0]); + EXPECT_EQ(points[1], encoder_features[1]); + EXPECT_EQ(points[2], encoder_features[2]); + EXPECT_EQ(points[3], encoder_features[3]); + + // The next three values are the relative coordinates with respect to the voxel average + EXPECT_EQ(0.0, encoder_features[4]); + EXPECT_EQ(0.0, encoder_features[5]); + EXPECT_EQ(0.0, encoder_features[6]); + + // The last two values are the relative coordinates with respect to the voxel center + float voxel_x_offset = voxel_size_x_ / 2 + voxel_idx * voxel_size_x_ + range_min_x_; + float voxel_y_offset = voxel_size_y_ / 2 + voxel_idy * voxel_size_y_ + range_min_y_; + + EXPECT_EQ(points[0] - voxel_x_offset, encoder_features[7]); + EXPECT_EQ(points[1] - voxel_y_offset, encoder_features[8]); +} + +TEST_F(PreprocessKernelTest, OutOfRangeTest) +{ + std::vector points{25.f, -61.1f, 100.f, 0.1f}; // 100.f is out of range + std::size_t points_num = 1; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(0, sum); +} + +TEST_F(PreprocessKernelTest, VoxelOverflowTest) +{ + std::array point{25.f, -61.1f, 1.8f, 0.1f}; + std::size_t points_num = 64; + std::vector points{}; + + for (std::size_t i = 0; i < points_num; ++i) { + points.insert(points.end(), point.begin(), point.end()); + } + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + // Note: due to atomic operations in the kernel, generateVoxels does not handle overflows in the + // counter, and instead is done in the following kernel + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Check that the voxel was filled + int voxel_idx = std::floor((points[0] - range_min_x_) / voxel_size_x_); + int voxel_idy = std::floor((points[1] - range_min_y_) / voxel_size_y_); + unsigned int voxel_index = voxel_idy * grid_size_x_ + voxel_idx; + + std::vector voxel_data{}; + voxel_data.resize((max_point_in_voxel_size_ + 1) * point_feature_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_data.data(), + voxels_buffer_d_.get() + voxel_index * max_point_in_voxel_size_ * point_feature_size_, + (max_point_in_voxel_size_ + 1) * point_feature_size_ * sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < max_point_in_voxel_size_; ++i) { + EXPECT_EQ(points[0], voxel_data[i * point_feature_size_ + 0]); + EXPECT_EQ(points[1], voxel_data[i * point_feature_size_ + 1]); + EXPECT_EQ(points[2], voxel_data[i * point_feature_size_ + 2]); + EXPECT_EQ(points[3], voxel_data[i * point_feature_size_ + 3]); + } + + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 0]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 1]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 2]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 3]); + + code = generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), grid_size_y_, grid_size_x_, max_voxel_size_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_); + + ASSERT_EQ(cudaSuccess, code); + + unsigned int num_pillars{}; + std::vector voxel_features(max_point_in_voxel_size_ * point_feature_size_, 0.f); + float num_voxels{}; + std::vector voxel_coordinates(3, 0); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_pillars, num_voxels_d_.get(), 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxels_d_.get(), + max_point_in_voxel_size_ * point_feature_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_voxels, num_points_per_voxel_d_.get(), 1 * sizeof(float), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_coordinates.data(), coordinates_d_.get(), 3 * sizeof(int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, num_pillars); + EXPECT_EQ(static_cast(max_point_in_voxel_size_), num_voxels); + EXPECT_EQ(0, voxel_coordinates[0]); + EXPECT_EQ(voxel_idy, voxel_coordinates[1]); + EXPECT_EQ(voxel_idx, voxel_coordinates[2]); + + for (std::size_t point_index = 0; point_index < max_point_in_voxel_size_; ++point_index) { + EXPECT_EQ(points[0], voxel_features[point_index * point_feature_size_ + 0]); + EXPECT_EQ(points[1], voxel_features[point_index * point_feature_size_ + 1]); + EXPECT_EQ(points[2], voxel_features[point_index * point_feature_size_ + 2]); + EXPECT_EQ(points[3], voxel_features[point_index * point_feature_size_ + 3]); + } + + code = generateFeatures_launch( + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), + max_voxel_size_, voxel_size_x_, voxel_size_y_, voxel_size_z_, range_min_x_, range_min_y_, + range_min_z_, encoder_in_features_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + std::vector encoder_features( + max_point_in_voxel_size_ * config_encoder_in_feature_size_, 0.f); + + CHECK_CUDA_ERROR(cudaMemcpy( + encoder_features.data(), encoder_in_features_d_.get(), + max_point_in_voxel_size_ * config_encoder_in_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost)); + + float voxel_x_offset = voxel_size_x_ / 2 + voxel_idx * voxel_size_x_ + range_min_x_; + float voxel_y_offset = voxel_size_y_ / 2 + voxel_idy * voxel_size_y_ + range_min_y_; + + for (std::size_t point_index = 0; point_index < max_point_in_voxel_size_; ++point_index) { + // The first four values are just the point features + EXPECT_EQ( + points[point_index * point_feature_size_ + 0], + encoder_features[point_index * config_encoder_in_feature_size_ + 0]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 1], + encoder_features[point_index * config_encoder_in_feature_size_ + 1]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 2], + encoder_features[point_index * config_encoder_in_feature_size_ + 2]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 3], + encoder_features[point_index * config_encoder_in_feature_size_ + 3]); + + // The next three values are the relative coordinates with respect to the voxel average + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 4], 1e-4); + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 5], 1e-4); + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 6], 1e-4); + + // The last two values are the relative coordinates with respect to the voxel center + EXPECT_EQ( + points[point_index * point_feature_size_ + 0] - voxel_x_offset, + encoder_features[point_index * config_encoder_in_feature_size_ + 7]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 1] - voxel_y_offset, + encoder_features[point_index * config_encoder_in_feature_size_ + 8]); + } +} + +} // namespace centerpoint + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp new file mode 100644 index 0000000000000..57ff51966a417 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp @@ -0,0 +1,79 @@ +// Copyright 2024 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 TEST_PREPROCESS_KERNEL_HPP_ +#define TEST_PREPROCESS_KERNEL_HPP_ + +#include + +#include + +namespace centerpoint +{ + +class PreprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + cudaStream_t stream_{nullptr}; + + std::size_t max_voxel_size_{}; + std::size_t max_point_in_voxel_size_{}; + std::size_t point_feature_size_{}; + std::size_t point_dim_size_{}; + std::size_t config_encoder_in_feature_size_{}; + std::size_t encoder_out_feature_size_{}; + std::size_t capacity_{}; + + float range_min_x_{}; + float range_min_y_{}; + float range_min_z_{}; + float range_max_x_{}; + float range_max_y_{}; + float range_max_z_{}; + float voxel_size_x_{}; + float voxel_size_y_{}; + float voxel_size_z_{}; + + std::size_t grid_size_x_{}; + std::size_t grid_size_y_{}; + std::size_t grid_size_z_{}; + + std::size_t voxels_size_{}; + std::size_t coordinates_size_{}; + std::size_t encoder_in_feature_size_{}; + std::size_t pillar_features_size_{}; + std::size_t spatial_features_size_{}; + std::size_t grid_xy_size_{}; + + std::size_t voxels_buffer_size_{}; + std::size_t mask_size_{}; + + cuda::unique_ptr voxels_d_{}; + cuda::unique_ptr coordinates_d_{}; + cuda::unique_ptr num_points_per_voxel_d_{}; + cuda::unique_ptr encoder_in_features_d_{}; + cuda::unique_ptr pillar_features_d_{}; + cuda::unique_ptr spatial_features_d_{}; + cuda::unique_ptr points_d_{}; + cuda::unique_ptr voxels_buffer_d_{}; + cuda::unique_ptr mask_d_{}; + cuda::unique_ptr num_voxels_d_{}; +}; + +} // namespace centerpoint + +#endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.cpp b/perception/lidar_centerpoint/test/test_voxel_generator.cpp new file mode 100644 index 0000000000000..5b0b3cd112e6c --- /dev/null +++ b/perception/lidar_centerpoint/test/test_voxel_generator.cpp @@ -0,0 +1,230 @@ +// Copyright 2024 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 "test_voxel_generator.hpp" + +#include + +#include + +void VoxelGeneratorTest::SetUp() +{ + // Setup things that should occur before every test instance should go here + node_ = std::make_shared("voxel_generator_test_node"); + + world_frame_ = "map"; + lidar_frame_ = "lidar"; + points_per_pointcloud_ = 10; + capacity_ = 100; + delta_pointcloud_x_ = 1.0; + + class_size_ = 5; + point_feature_size_ = 4; + max_voxel_size_ = 100000000; + point_cloud_range_ = std::vector{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; + voxel_size_ = std::vector{0.32, 0.32, 10.0}; + downsample_factor_ = 1; + encoder_in_feature_size_ = 9; + score_threshold_ = 0.35f; + circle_nms_dist_threshold_ = 0.5f; + yaw_norm_thresholds_ = std::vector{0.5, 0.5, 0.5}; + has_variance_ = false; + + cloud1_ = std::make_unique(); + cloud2_ = std::make_unique(); + + cloud1_->header.frame_id = lidar_frame_; + + // Set up the fields for x, y, and z coordinates + cloud1_->fields.resize(3); + sensor_msgs::PointCloud2Modifier modifier(*cloud1_); + modifier.setPointCloud2FieldsByString(1, "xyz"); + + // Resize the cloud to hold points_per_pointcloud_ points + modifier.resize(points_per_pointcloud_); + + // Create an iterator for the x, y, z fields + sensor_msgs::PointCloud2Iterator iter_x(*cloud1_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud1_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud1_, "z"); + + // Populate the point cloud + for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(i); + *iter_y = static_cast(i); + *iter_z = static_cast(i); + } + + *cloud2_ = *cloud1_; + + // Set the stamps for the point clouds. They usually come every 100ms + cloud1_->header.stamp.sec = 1; + cloud1_->header.stamp.nanosec = 100'000'000; + cloud2_->header.stamp.sec = 1; + cloud2_->header.stamp.nanosec = 200'000'000; + + tf2_buffer_ = std::make_unique(node_->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + + // The vehicle moves 1m/s in the x direction + const double world_origin_x = 6'371'000.0; + const double world_origin_y = 1'371'000.0; + + transform1_.header.stamp = cloud1_->header.stamp; + transform1_.header.frame_id = world_frame_; + transform1_.child_frame_id = lidar_frame_; + transform1_.transform.translation.x = world_origin_x; + transform1_.transform.translation.y = world_origin_y; + transform1_.transform.translation.z = 1.8; + transform1_.transform.rotation.x = 0.0; + transform1_.transform.rotation.y = 0.0; + transform1_.transform.rotation.z = 0.0; + transform1_.transform.rotation.w = 1.0; + + transform2_ = transform1_; + transform2_.header.stamp = cloud2_->header.stamp; + transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; +} + +void VoxelGeneratorTest::TearDown() +{ +} + +TEST_F(VoxelGeneratorTest, SingleFrame) +{ + const unsigned int num_past_frames = 0; // only current frame + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_TRUE(status1); + EXPECT_EQ(points_per_pointcloud_, generated_points_num); + + // Check valid points + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 2]); + EXPECT_EQ(static_cast(0), points[i * config.point_feature_size_ + 3]); + } + + // Check invalid points + for (std::size_t i = points_per_pointcloud_ * config.point_feature_size_; + i < capacity_ * config.point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +TEST_F(VoxelGeneratorTest, TwoFramesNoTf) +{ + const unsigned int num_past_frames = 1; + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_FALSE(status1); + EXPECT_FALSE(status2); + EXPECT_EQ(0, generated_points_num); +} + +TEST_F(VoxelGeneratorTest, TwoFrames) +{ + const unsigned int num_past_frames = 1; + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + tf2_buffer_->setTransform(transform1_, "authority1"); + tf2_buffer_->setTransform(transform2_, "authority1"); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_TRUE(status1); + EXPECT_TRUE(status2); + EXPECT_EQ(2 * points_per_pointcloud_, generated_points_num); + + // Check valid points for the latest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 2]); + EXPECT_EQ(static_cast(0), points[i * config.point_feature_size_ + 3]); + } + + // Check valid points for the oldest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are tf conversions, so results are not numerically the same + EXPECT_NEAR( + static_cast(i) - delta_pointcloud_x_, + points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 0], 1e-6); + EXPECT_NEAR( + static_cast(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 1], + 1e-6); + EXPECT_NEAR( + static_cast(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 2], + 1e-6); + EXPECT_NEAR(0.1, points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 3], 1e-6); + } + + // Check invalid points + for (std::size_t i = 2 * points_per_pointcloud_ * config.point_feature_size_; + i < capacity_ * config.point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.hpp b/perception/lidar_centerpoint/test/test_voxel_generator.hpp new file mode 100644 index 0000000000000..8fb7d8dffa44d --- /dev/null +++ b/perception/lidar_centerpoint/test/test_voxel_generator.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 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 TEST_VOXEL_GENERATOR_HPP_ +#define TEST_VOXEL_GENERATOR_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include + +class VoxelGeneratorTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + // These need to be public so that they can be accessed in the test cases + rclcpp::Node::SharedPtr node_; + + std::unique_ptr cloud1_, cloud2_; + geometry_msgs::msg::TransformStamped transform1_, transform2_; + + std::unique_ptr densification_param_ptr_; + std::unique_ptr config_ptr_; + + std::unique_ptr tf2_buffer_; + + std::string world_frame_; + std::string lidar_frame_; + std::size_t points_per_pointcloud_; + std::size_t capacity_; + double delta_pointcloud_x_; + + std::size_t class_size_; + float point_feature_size_; + std::size_t max_voxel_size_; + + std::vector point_cloud_range_; + std::vector voxel_size_; + std::size_t downsample_factor_; + std::size_t encoder_in_feature_size_; + float score_threshold_; + float circle_nms_dist_threshold_; + std::vector yaw_norm_thresholds_; + bool has_variance_; +}; + +#endif // TEST_VOXEL_GENERATOR_HPP_ diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index a1b776bdb6393..a5b7b0ad6be01 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -1,7 +1,10 @@ /**: ros__parameters: enable_delay_compensation: true - prediction_time_horizon: 10.0 #[s] + prediction_time_horizon: + vehicle: 15.0 #[s] + pedestrian: 10.0 #[s] + unknown: 10.0 #[s] lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 213f18d63ef3a..93cdb24f91321 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -106,6 +106,14 @@ struct PredictedRefPath Maneuver maneuver; }; +struct PredictionTimeHorizon +{ + // NOTE(Mamoru Sobue): motorcycle belongs to "vehicle" and bicycle to "pedestrian" + double vehicle; + double pedestrian; + double unknown; +}; + using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; using autoware_auto_mapping_msgs::msg::HADMapBin; @@ -170,7 +178,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; - double prediction_time_horizon_; + PredictionTimeHorizon prediction_time_horizon_; double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; @@ -253,7 +261,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::string & object_id, std::unordered_map & current_users); std::vector getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time); + const double object_detected_time, const double time_horizon); Maneuver predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 5023051da5e71..3c1a5432d3b08 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -80,21 +80,24 @@ using PosePath = std::vector; class PathGenerator { public: - PathGenerator( - const double time_horizon, const double lateral_time_horizon, - const double sampling_time_interval, const double min_crosswalk_user_velocity); + PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); - PredictedPath generatePathForNonVehicleObject(const TrackedObject & object) const; + PredictedPath generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const; - PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; + PredictedPath generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const; - PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object) const; + PredictedPath generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const; PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; PredictedPath generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const; PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; @@ -111,22 +114,21 @@ class PathGenerator private: // Parameters - double time_horizon_; - double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; bool use_vehicle_acceleration_; double acceleration_exponential_half_life_; // Member functions - PredictedPath generateStraightPath(const TrackedObject & object) const; + PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; PredictedPath generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, - const double max_length) const; + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const; Eigen::Vector3d calcLatCoefficients( const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; Eigen::Vector2d calcLonCoefficients( @@ -140,7 +142,8 @@ class PathGenerator const PosePath & ref_path) const; FrenetPoint getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit = 0.0) const; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/perception/map_based_prediction/schema/map_based_prediction.schema.json index 8ddb122ebb56e..8e5ef9e542f54 100644 --- a/perception/map_based_prediction/schema/map_based_prediction.schema.json +++ b/perception/map_based_prediction/schema/map_based_prediction.schema.json @@ -12,9 +12,23 @@ "description": "flag to enable the time delay compensation for the position of the object" }, "prediction_time_horizon": { - "type": "number", - "default": "10.0", - "description": "predict time duration for predicted path" + "properties": { + "vehicle": { + "type": "number", + "default": "15.0", + "description": "predict time duration for predicted path of vehicle" + }, + "pedestrian": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of pedestrian" + }, + "unknown": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of unknown" + } + } }, "lateral_control_time_horizon": { "type": "number", diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index b55ea72402d74..9b527fafb30e9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -766,7 +766,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ google::InstallFailureSignalHandler(); } enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_horizon_.vehicle = declare_parameter("prediction_time_horizon.vehicle"); + prediction_time_horizon_.pedestrian = + declare_parameter("prediction_time_horizon.pedestrian"); + prediction_time_horizon_.unknown = declare_parameter("prediction_time_horizon.unknown"); lateral_control_time_horizon_ = declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); @@ -840,8 +843,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); path_generator_ = std::make_shared( - prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, - min_crosswalk_user_velocity_); + prediction_sampling_time_interval_, min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); @@ -1050,8 +1052,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // For off lane obstacles if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1066,8 +1068,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt transformed_object.kinematics.twist_with_covariance.twist.linear.x, transformed_object.kinematics.twist_with_covariance.twist.linear.y); if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1079,13 +1081,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Get Predicted Reference Path for Each Maneuver and current lanelets // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + const auto ref_paths = getPredictedReferencePath( + transformed_object, current_lanelets, objects_detected_time, + prediction_time_horizon_.vehicle); // If predicted reference path is empty, assume this object is out of the lane if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1120,7 +1123,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); + yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle, + lateral_control_time_horizon_, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1183,8 +1187,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } default: { auto predicted_unknown_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject( + transformed_object, prediction_time_horizon_.unknown); predicted_path.confidence = 1.0; predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1337,7 +1341,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( { auto predicted_object = convertToPredictedObject(object); { - PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1388,7 +1393,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_ * 2.0, + edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); @@ -1398,7 +1403,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_ * 2.0, + edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); @@ -1422,27 +1427,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto reachable_first = hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.front_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); const auto reachable_second = hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.back_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); if (!reachable_first && !reachable_second) { continue; } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { continue; } - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( + object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) { @@ -1740,7 +1745,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time) + const double object_detected_time, const double time_horizon) { const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1752,7 +1757,7 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.acceleration_with_covariance.accel.linear.x, object.kinematics.acceleration_with_covariance.accel.linear.y) : 0.0; - const double t_h = prediction_time_horizon_; + const double t_h = time_horizon; const double λ = std::log(2) / acceleration_exponential_half_life_; auto get_search_distance_with_decaying_acc = [&]() -> double { diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4238859535c8e..838d7b1c8e316 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,18 +23,16 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity) -: time_horizon_(time_horizon), - lateral_time_horizon_(lateral_time_horizon), - sampling_time_interval_(sampling_time_interval), + const double sampling_time_interval, const double min_crosswalk_user_velocity) +: sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { } -PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathToTargetPoint( @@ -75,7 +73,8 @@ PredictedPath PathGenerator::generatePathToTargetPoint( } PredictedPath PathGenerator::generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const { PredictedPath predicted_path{}; const double ep = 0.001; @@ -99,7 +98,7 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; if (dt < arrival_time) { world_frame_pose.position.x = @@ -131,39 +130,43 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( return predicted_path; } -PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const { PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { path.path.push_back(object.kinematics.pose_with_covariance.pose); } return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit) const { if (ref_paths.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit); + return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); } -PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const +PredictedPath PathGenerator::generateStraightPath( + const TrackedObject & object, const double longitudinal_duration) const { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; constexpr double ep = 0.001; - const double duration = time_horizon_ + ep; + const double duration = longitudinal_duration + ep; PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -178,11 +181,12 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit) const { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path, speed_limit); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -196,13 +200,13 @@ PredictedPath PathGenerator::generatePolynomialPath( // Step2. Generate Predicted Path on a Frenet coordinate const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len); + generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); // Step3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate @@ -210,12 +214,10 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, - const double max_length) const + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const { FrenetPath path; - const double duration = time_horizon_; - const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory const Eigen::Vector3d lat_coeff = @@ -387,7 +389,8 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit) const { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -413,7 +416,7 @@ FrenetPoint PathGenerator::getFrenetPoint( : 0.0; // Using a decaying acceleration model. Consult the README for more information about the model. - const double t_h = time_horizon_; + const double t_h = duration; const float λ = std::log(2) / acceleration_exponential_half_life_; auto have_same_sign = [](double a, double b) -> bool { diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp index 555dc217fb5ed..6134f0c6a25d7 100644 --- a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp +++ b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -52,19 +52,17 @@ TEST(PathGenerator, test_generatePathForNonVehicleObject) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate pedestrian object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); // Generate predicted path const PredictedPath predicted_path = - path_generator.generatePathForNonVehicleObject(tracked_object); + path_generator.generatePathForNonVehicleObject(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -77,19 +75,17 @@ TEST(PathGenerator, test_generatePathForLowSpeedVehicle) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); // Generate predicted path const PredictedPath predicted_path = - path_generator.generatePathForLowSpeedVehicle(tracked_object); + path_generator.generatePathForLowSpeedVehicle(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -102,17 +98,16 @@ TEST(PathGenerator, test_generatePathForOffLaneVehicle) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); - const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object); + const PredictedPath predicted_path = + path_generator.generatePathForOffLaneVehicle(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -129,8 +124,7 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -144,8 +138,8 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) ref_paths.push_back(pose); // Generate predicted path - const PredictedPath predicted_path = - path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths); + const PredictedPath predicted_path = path_generator.generatePathForOnLaneVehicle( + tracked_object, ref_paths, prediction_time_horizon, lateral_control_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -158,12 +152,10 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); @@ -178,8 +170,8 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) reachable_crosswalk.back_left_point << -1.0, 1.0; // Generate predicted path - const PredictedPath predicted_path = - path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk); + const PredictedPath predicted_path = path_generator.generatePathForCrosswalkUser( + tracked_object, reachable_crosswalk, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -191,13 +183,10 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) TEST(PathGenerator, test_generatePathToTargetPoint) { // Generate Path generator - const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); diff --git a/planning/.pages b/planning/.pages index 28a791801b805..4514894605bbc 100644 --- a/planning/.pages +++ b/planning/.pages @@ -63,6 +63,10 @@ nav: - 'Surround Obstacle Checker': - 'About Surround Obstacle Checker': planning/surround_obstacle_checker - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'Motion Velocity Planner': + - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ + - 'Available Modules': + - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - 'Motion Velocity Smoother': - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 2c181489ba4c2..e5c3a76fd1c45 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,6 +17,7 @@ turn_signal_minimum_search_distance: 10.0 turn_signal_search_time: 3.0 turn_signal_shift_length_threshold: 0.3 + turn_signal_remaining_shift_length_threshold: 0.1 turn_signal_on_swerving: true enable_akima_spline_first: 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 b56d1a207f76d..f7d204d2bff87 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -260,6 +260,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_search_time = declare_parameter("turn_signal_search_time"); p.turn_signal_shift_length_threshold = declare_parameter("turn_signal_shift_length_threshold"); + p.turn_signal_remaining_shift_length_threshold = + declare_parameter("turn_signal_remaining_shift_length_threshold"); p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving"); p.enable_akima_spline_first = declare_parameter("enable_akima_spline_first"); diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md index bfc22bacc8ede..4302b74dcf2c6 100644 --- a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md @@ -20,14 +20,15 @@ Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in c ## Parameters for turn signal decider -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :--------------------------------------------------------------------------- | :------------ | -| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | -| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | -| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | -| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | -| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | -| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | +| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | +| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | +| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | +| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | +| turn_signal_remaining_shift_length_threshold | [m] | double | When the ego's current shift length minus its end shift length is less than this threshold, the turn signal will be turned off. | 0.1 | +| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | Note that the default values for `turn_signal_intersection_search_distance` and `turn_signal_search_time` is strictly followed by [Japanese Road Traffic Laws](https://www.japaneselawtranslation.go.jp/ja/laws/view/2962). So if your country does not allow to use these default values, you should change these values in configuration files. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 1241f98daa747..dbe27d856bc40 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -54,6 +54,7 @@ struct BehaviorPathPlannerParameters double turn_signal_search_time; double turn_signal_minimum_search_distance; double turn_signal_shift_length_threshold; + double turn_signal_remaining_shift_length_threshold; bool turn_signal_on_swerving; bool enable_akima_spline_first; diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 21937aa76da29..fb15391550980 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -615,7 +615,6 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - constexpr double THRESHOLD = 0.1; const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -674,7 +673,9 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( } // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) { + if ( + std::fabs(end_shift_length - current_shift_length) < + p.turn_signal_remaining_shift_length_threshold) { return std::make_pair(TurnSignalInfo{}, true); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..b96ca3017a031 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_out_of_lane_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_filter_predicted_objects.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md new file mode 100644 index 0000000000000..99396eb0edf22 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -0,0 +1,165 @@ +## Out of Lane + +### Role + +`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects. + +### Activation Timing + +This module is activated if `launch_out_of_lane` is set to true. + +### Inner-workings / Algorithms + +The algorithm is made of the following steps. + +1. Calculate the ego path footprints (red). +2. Calculate the other lanes (magenta). +3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green). +4. For each overlapping range, decide if a stop or slow down action must be taken. +5. For each action, insert the corresponding stop or slow down point in the path. + +![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png) + +#### 1. Ego Path Footprints + +In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters. + +#### 2. Other lanes + +In the second step, the set of lanes to consider for overlaps is generated. +This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. +The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. + +A lanelet is deemed non-relevant if it meets one of the following conditions. + +- It is part of the lanelets followed by the ego path. +- It contains the rear point of the ego footprint. +- It follows one of the ego path lanelets. + +#### 3. Overlapping ranges + +In the third step, overlaps between the ego path footprints and the other lanes are calculated. +For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`. +For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored. +Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored. +Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information: + +- overlapped other lane $l$. +- start and end ego path indexes. +- start and end ego path arc lengths. +- start and end overlap points. + +#### 4. Decisions + +In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects. +The conditions for the decision depend on the value of the `mode` parameter. + +Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path). +If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. +If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. + + + + + + +
+ + + +
+ +##### Threshold + +With the `mode` set to `"threshold"`, +a decision to stop or slow down before a range is made if +an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`. + +##### TTC (time to collision) + +With the `mode` set to `"ttc"`, +estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated. +This is then used to calculate the time to collision over the period where ego crosses the overlap. +If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made. + +##### Intervals + +With the `mode` set to `"intervals"`, +the estimated times when ego and the dynamic objects reach the start and end points of +the overlapping range are used to create time intervals. +These intervals can be made shorter or longer using the +`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters. +If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made. + +##### Time estimates + +###### Ego + +To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path +at its current velocity or at half the velocity of the path points, whichever is higher. + +###### Dynamic objects + +Two methods are used to estimate the time when a dynamic objects with reach some point. +If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. +Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. + +#### 5. Path update + +Finally, for each decision to stop or slow down before an overlapping range, +a point is inserted in the path. +For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$, +a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$. +Such point with no overlap must exist since, by definition of the overlapping range, +we know that there is no overlap at $i-1$. + +If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter), +it is skipped. + +Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible. + +### Module Parameters + +| Parameter | Type | Description | +| ----------------------------- | ------ | --------------------------------------------------------------------------------- | +| `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc | +| `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane | + +| Parameter /threshold | Type | Description | +| -------------------- | ------ | ---------------------------------------------------------------- | +| `time_threshold` | double | [s] consider objects that will reach an overlap within this time | + +| Parameter /intervals | Type | Description | +| --------------------- | ------ | ------------------------------------------------------- | +| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | +| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | + +| Parameter /ttc | Type | Description | +| -------------- | ------ | ------------------------------------------------------------------------------------------------------ | +| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | + +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | + +| Parameter /overlap | Type | Description | +| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | +| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | +| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | + +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | + +| Parameter /ego | Type | Description | +| -------------------- | ------ | ---------------------------------------------------- | +| `extra_front_offset` | double | [m] extra front distance to add to the ego footprint | +| `extra_rear_offset` | double | [m] extra rear distance to add to the ego footprint | +| `extra_left_offset` | double | [m] extra left distance to add to the ego footprint | +| `extra_right_offset` | double | [m] extra right distance to add to the ego footprint | diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml new file mode 100644 index 0000000000000..e57b5a45d8be6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -0,0 +1,43 @@ +/**: + ros__parameters: + out_of_lane: # module to stop or slowdown before overlapping another lane with other objects + mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" + skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + + threshold: + time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time + intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego + ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer + objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer + ttc: + threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap + + objects: + minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored + use_predicted_paths: true # if true, use the predicted paths to estimate future positions. + # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. + distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights + + overlap: + minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered + extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) + + action: # action to insert in the trajectory if an object causes a conflict at an overlap + skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed + precision: 0.1 # [m] precision when inserting a stop pose in the trajectory + distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled + slowdown: + distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap + velocity: 2.0 # [m/s] slowdown velocity + stop: + distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap + + ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego + extra_front_offset: 0.0 # [m] extra front distance + extra_rear_offset: 0.0 # [m] extra rear distance + extra_right_offset: 0.0 # [m] extra right distance + extra_left_offset: 0.0 # [m] extra left distance diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png new file mode 100644 index 0000000000000..f095467b3b0ac Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png new file mode 100644 index 0000000000000..2f358975b9491 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png new file mode 100644 index 0000000000000..fdb75ac0c6eb8 Binary files /dev/null and b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml new file mode 100644 index 0000000000000..5ba740773d5a5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_motion_velocity_out_of_lane_module + 0.1.0 + The motion_velocity_out_of_lane_module package + + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + + Apache License 2.0 + + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_motion_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + traffic_light_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml new file mode 100644 index 0000000000000..0a05163e8f1d0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp new file mode 100644 index 0000000000000..157d545dcac0d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -0,0 +1,85 @@ +// Copyright 2024 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 "calculate_slowdown_points.hpp" + +#include "footprint.hpp" + +#include +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +{ + // TODO(Maxime): use the library function + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); + const auto to_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0, decision.target_trajectory_idx); + TrajectoryPoint interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || prev_slowdown_point || + can_decelerate(ego_data, interpolated_point, decision.velocity); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose); + const auto is_overlap_lane = boost::geometry::overlaps( + interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); + const auto is_overlap_extra_lane = + prev_slowdown_point && + boost::geometry::overlaps( + interpolated_footprint, + prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); + if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) + return interpolated_point; + } + return std::nullopt; +} + +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..19ed066548d0f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 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 CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief estimate whether ego can decelerate without breaking the deceleration limit +/// @details assume ego wants to reach the target point at the target velocity +/// @param [in] ego_data ego data +/// @param [in] point target point +/// @param [in] target_vel target_velocity +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel); + +/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid +/// @param [in] ego_data ego data +/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) +/// @param [in] footprint the ego footprint +/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits +/// @param [in] params parameters +/// @return the last pose that is not out of lane (if found) +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params); + +/// @brief calculate the slowdown point to insert in the trajectory +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param prev_slowdown_point previously calculated slowdown point +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params); +} // namespace autoware::motion_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp new file mode 100644 index 0000000000000..585f4760290ef --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -0,0 +1,242 @@ +// Copyright 2024 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 "debug.hpp" + +#include "types.hpp" + +#include + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +namespace +{ + +visualization_msgs::msg::Marker get_base_marker() +{ + visualization_msgs::msg::Marker base_marker; + base_marker.header.frame_id = "map"; + base_marker.header.stamp = rclcpp::Time(0); + base_marker.id = 0; + base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + base_marker.action = visualization_msgs::msg::Marker::ADD; + base_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + return base_marker; +} +void add_footprint_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "footprints"; + for (const auto & f : footprints) { + debug_marker.points.clear(); + for (const auto & p : f) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_current_overlap_marker( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::BasicPolygon2d & current_footprint, + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "current_overlap"; + debug_marker.points.clear(); + for (const auto & p : current_footprint) + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); + if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); + if (current_overlapped_lanelets.empty()) + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); + else + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + for (const auto & ll : current_overlapped_lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon3d()) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_lanelet_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::ConstLanelets & lanelets, const std::string & ns, + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = ns; + debug_marker.color = color; + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + + // add a small z offset to draw above the lanelet map + for (const auto & p : ll.polygon3d()) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const std::vector & trajectory_points, + const size_t first_ego_idx, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "ranges"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + for (const auto & range : ranges) { + debug_marker.points.clear(); + debug_marker.points.push_back( + trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + for (const auto & overlap : range.debug.overlaps) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + } + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.exiting_point.x(), range.exiting_point.y(), z)); + debug_marker.points.push_back( + trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_decision_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.action = debug_marker.ADD; + debug_marker.id = 0; + debug_marker.ns = "decisions"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.points.clear(); + for (const auto & range : ranges) { + debug_marker.type = debug_marker.LINE_STRIP; + if (range.debug.decision) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + debug_marker.points.push_back( + range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + } + debug_marker_array.markers.push_back(debug_marker); + debug_marker.points.clear(); + debug_marker.id++; + + debug_marker.type = debug_marker.TEXT_VIEW_FACING; + debug_marker.pose.position.x = range.entering_point.x(); + debug_marker.pose.position.y = range.entering_point.y(); + debug_marker.pose.position.z = z; + std::stringstream ss; + ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time + << "\n"; + if (range.debug.object) { + debug_marker.pose.position.x += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; + debug_marker.pose.position.y += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; + debug_marker.pose.position.x /= 2; + debug_marker.pose.position.y /= 2; + ss << "Obj: " << range.debug.times.object.enter_time << " - " + << range.debug.times.object.exit_time << "\n"; + } + debug_marker.scale.z = 1.0; + debug_marker.text = ss.str(); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { + debug_marker_array.markers.push_back(debug_marker); + } +} +} // namespace +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) +{ + constexpr auto z = 0.0; + visualization_msgs::msg::MarkerArray debug_marker_array; + + debug::add_footprint_markers( + debug_marker_array, debug_data.footprints, z, debug_data.prev_footprints); + debug::add_current_overlap_marker( + debug_marker_array, debug_data.current_footprint, debug_data.current_overlapped_lanelets, z, + debug_data.prev_current_overlapped_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", + tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), + debug_data.prev_trajectory_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", + tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data.prev_ignored_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.other_lanelets, "other_lanelets", + tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data.prev_other_lanelets); + debug::add_range_markers( + debug_marker_array, debug_data.ranges, debug_data.trajectory_points, + debug_data.first_trajectory_idx, z, debug_data.prev_ranges); + debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + return debug_marker_array; +} + +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params) +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.text = "out_of_lane"; + wall.longitudinal_offset = params.front_offset; + wall.style = motion_utils::VirtualWallType::slowdown; + for (const auto & slowdown : debug_data.slowdowns) { + wall.pose = slowdown.point.pose; + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp new file mode 100644 index 0000000000000..4a5be9dfb07ac --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -0,0 +1,31 @@ +// Copyright 2024 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 DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane::debug + +#endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp new file mode 100644 index 0000000000000..56047e459bf51 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -0,0 +1,388 @@ +// Copyright 2024 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 "decisions.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +namespace autoware::motion_velocity_planner::out_of_lane +{ +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) +{ + return motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); +} + +double time_along_trajectory( + const EgoData & ego_data, const size_t target_idx, const double min_velocity) +{ + const auto dist = distance_along_trajectory(ego_data, target_idx); + const auto v = std::max( + std::max(ego_data.velocity, min_velocity), + ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] + .longitudinal_velocity_mps * + 0.5); + return dist / v; +} + +bool object_is_incoming( + const lanelet::BasicPoint2d & object_position, + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lane) +{ + const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); + if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; + for (const auto & lls : lanelets) + for (const auto & ll : lls) + if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; + return false; +} + +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger) +{ + // skip the dynamic object if it is not in a lane preceding the overlapped lane + // lane changes are intentionally not considered + const auto object_point = lanelet::BasicPoint2d( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; + + const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; + const auto max_lon_deviation = object.shape.dimensions.x / 2.0; + auto worst_enter_time = std::optional(); + auto worst_exit_time = std::optional(); + + for (const auto & predicted_path : object.kinematics.predicted_paths) { + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + if (unique_path_points.size() < 2) continue; + const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); + const auto enter_point = + geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto enter_segment_idx = + motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); + const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( + unique_path_points, enter_segment_idx, enter_point); + const auto enter_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); + const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( + unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); + const auto enter_offset_ratio = enter_offset / enter_segment_length; + const auto enter_time = + static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; + + const auto exit_point = + geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto exit_segment_idx = + motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); + const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( + unique_path_points, exit_segment_idx, exit_point); + const auto exit_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); + const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( + unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); + const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); + const auto exit_time = + static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; + + RCLCPP_DEBUG( + logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, + enter_time, exit_time); + // predicted path is too far from the overlapping range to be relevant + const auto is_far_from_entering_point = enter_lat_dist > max_deviation; + const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; + if (is_far_from_entering_point && is_far_from_exiting_point) { + RCLCPP_DEBUG( + logger, + " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", + is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, + max_deviation); + continue; + } + const auto is_last_predicted_path_point = + (exit_segment_idx + 2 == unique_path_points.size() || + enter_segment_idx + 2 == unique_path_points.size()); + const auto does_not_reach_overlap = + is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); + if (does_not_reach_overlap) { + RCLCPP_DEBUG( + logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", + std::min(exit_offset, enter_offset), max_lon_deviation); + continue; + } + + const auto same_driving_direction_as_ego = enter_time < exit_time; + if (same_driving_direction_as_ego) { + worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; + worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; + } else { + worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; + worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; + } + } + if (worst_enter_time && worst_exit_time) { + RCLCPP_DEBUG( + logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, + *worst_exit_time); + return std::make_pair(*worst_enter_time, *worst_exit_time); + } + RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); + return {}; +} + +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const DecisionInputs & inputs, const rclcpp::Logger & logger) +{ + const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; + const auto object_point = lanelet::BasicPoint2d(p.x, p.y); + const auto half_size = object.shape.dimensions.x / 2.0; + lanelet::ConstLanelets object_lanelets; + for (const auto & ll : inputs.lanelets) + if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) + object_lanelets.push_back(ll); + + geometry_msgs::msg::Pose pose; + pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; + pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; + const auto range_size = std::abs(range_enter_length - range_exit_length); + auto worst_enter_dist = std::optional(); + auto worst_exit_dist = std::optional(); + for (const auto & lane : object_lanelets) { + const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); + RCLCPP_DEBUG( + logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); + if (path) { + lanelet::ConstLanelets lls; + for (const auto & ll : *path) lls.push_back(ll); + pose.position.set__x(object_point.x()).set__y(object_point.y()); + const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; + pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto enter_dist = + lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; + pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto exit_dist = + lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; + RCLCPP_DEBUG( + logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, + enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, + range.exiting_point.x(), range.exiting_point.y()); + const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; + if (already_entered_range) continue; + // multiple paths to the overlap -> be conservative and use the "worst" case + // "worst" = min/max arc length depending on if the lane is running opposite to the ego + // trajectory + const auto is_opposite = enter_dist > exit_dist; + if (!worst_enter_dist) + worst_enter_dist = enter_dist; + else if (is_opposite) + worst_enter_dist = std::max(*worst_enter_dist, enter_dist); + else + worst_enter_dist = std::min(*worst_enter_dist, enter_dist); + if (!worst_exit_dist) + worst_exit_dist = exit_dist; + else if (is_opposite) + worst_exit_dist = std::max(*worst_exit_dist, exit_dist); + else + worst_exit_dist = std::min(*worst_exit_dist, exit_dist); + } + } + if (worst_enter_dist && worst_exit_dist) { + const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; + return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); + } + return {}; +} + +bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) +{ + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; +} + +bool intervals_condition( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + const auto opposite_way_condition = [&]() -> bool { + const auto ego_exits_before_object_enters = + range_times.ego.exit_time + params.intervals_ego_buffer < + range_times.object.enter_time + params.intervals_obj_buffer; + RCLCPP_DEBUG( + logger, + "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " + "enter = %d\n", + range_times.ego.exit_time + params.intervals_ego_buffer, + range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); + return ego_exits_before_object_enters; + }; + const auto same_way_condition = [&]() -> bool { + const auto object_enters_during_overlap = + range_times.ego.enter_time - params.intervals_ego_buffer < + range_times.object.enter_time + params.intervals_obj_buffer && + range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < + range_times.ego.exit_time + params.intervals_ego_buffer; + const auto object_exits_during_overlap = + range_times.ego.enter_time - params.intervals_ego_buffer < + range_times.object.exit_time + params.intervals_obj_buffer && + range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < + range_times.ego.exit_time + params.intervals_ego_buffer; + RCLCPP_DEBUG( + logger, + "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " + "not " + "enter = %d\n", + object_enters_during_overlap, object_exits_during_overlap, + object_enters_during_overlap || object_exits_during_overlap); + return object_enters_during_overlap || object_exits_during_overlap; + }; + + const auto object_is_going_same_way = + range_times.object.enter_time < range_times.object.exit_time; + return (object_is_going_same_way && same_way_condition()) || + (!object_is_going_same_way && opposite_way_condition()); +} + +bool ttc_condition( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; + const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; + const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); + const auto ttc_is_bellow_threshold = + std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; + RCLCPP_DEBUG( + logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, + (collision_during_overlap || ttc_is_bellow_threshold)); + return collision_during_overlap || ttc_is_bellow_threshold; +} + +bool will_collide_on_range( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + RCLCPP_DEBUG( + logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, + range_times.object.exit_time); + return (params.mode == "threshold" && threshold_condition(range_times, params)) || + (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || + (params.mode == "ttc" && ttc_condition(range_times, params, logger)); +} + +bool should_not_enter( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger) +{ + RangeTimes range_times{}; + range_times.ego.enter_time = + time_along_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); + RCLCPP_DEBUG( + logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", + range.entering_trajectory_idx, range.exiting_trajectory_idx, range.lane.id(), + range_times.ego.enter_time, range_times.ego.exit_time); + for (const auto & object : inputs.objects.objects) { + RCLCPP_DEBUG( + logger, "\t\t[%s] going at %2.2fm/s", + tier4_autoware_utils::toHexString(object.object_id).c_str(), + object.kinematics.initial_twist_with_covariance.twist.linear.x); + if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { + RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); + continue; // skip objects with velocity bellow a threshold + } + // skip objects that are already on the interval + const auto enter_exit_time = + params.objects_use_predicted_paths + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_dist_buffer, logger) + : object_time_to_range(object, range, inputs, logger); + if (!enter_exit_time) { + RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); + continue; // skip objects that are not driving towards the overlapping range + } + + range_times.object.enter_time = enter_exit_time->first; + range_times.object.exit_time = enter_exit_time->second; + if (will_collide_on_range(range_times, params, logger)) { + range.debug.times = range_times; + range.debug.object = object; + return true; + } + } + range.debug.times = range_times; + return false; +} + +void set_decision_velocity( + std::optional & decision, const double distance, const PlannerParam & params) +{ + if (distance < params.stop_dist_threshold) { + decision->velocity = 0.0; + } else if (distance < params.slow_dist_threshold) { + decision->velocity = params.slow_velocity; + } else { + decision.reset(); + } +} + +std::optional calculate_decision( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger) +{ + std::optional decision; + if (should_not_enter(range, inputs, params, logger)) { + decision.emplace(); + decision->target_trajectory_idx = inputs.ego_data.first_trajectory_idx + + range.entering_trajectory_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; + const auto ego_dist_to_range = + distance_along_trajectory(inputs.ego_data, range.entering_trajectory_idx); + set_decision_velocity(decision, ego_dist_to_range, params); + } + return decision; +} + +std::vector calculate_decisions( + const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) +{ + std::vector decisions; + for (const auto & range : inputs.ranges) { + if (range.entering_trajectory_idx == 0UL) continue; // skip if we already entered the range + const auto optional_decision = calculate_decision(range, inputs, params, logger); + range.debug.decision = optional_decision; + if (optional_decision) decisions.push_back(*optional_decision); + } + return decisions; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp new file mode 100644 index 0000000000000..2aa3a8b490bf6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 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 DECISIONS_HPP_ +#define DECISIONS_HPP_ + +#include "types.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief calculate the distance along the ego trajectory between ego and some target trajectory +/// index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @return distance between ego and the target [m] +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief estimate the time when ego will reach some target trajectory index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @param [in] min_velocity minimum ego velocity used to estimate the time +/// @return time taken by ego to reach the target [s] +double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit +/// points of an overlapping range +/// @details times when the predicted paths of the object enters/exits the range are calculated +/// but may not exist (e.g,, predicted path ends before reaching the end of the range) +/// @param [in] object dynamic object +/// @param [in] range overlapping range +/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] logger ros logger +/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in +/// the opposite direction, time at enter > time at exit +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger); +/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit +/// points of an overlapping range +/// @param [in] object dynamic object +/// @param [in] range overlapping range +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range +/// @param [in] logger ros logger +/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in +/// the opposite direction, time at enter > time at exit. +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const DecisionInputs & inputs, const rclcpp::Logger & logger); +/// @brief decide whether an object is coming in the range at the same time as ego +/// @details the condition depends on the mode (threshold, intervals, ttc) +/// @param [in] range_times times when ego and the object enter/exit the range +/// @param [in] params parameters +/// @param [in] logger ros logger +bool will_collide_on_range( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); +/// @brief check whether we should avoid entering the given range +/// @param [in] range the range to check +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return true if we should avoid entering the range +bool should_not_enter( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger); +/// @brief set the velocity of a decision (or unset it) based on the distance away from the range +/// @param [out] decision decision to update (either set its velocity or unset the decision) +/// @param [in] distance distance between ego and the range corresponding to the decision +/// @param [in] params parameters +void set_decision_velocity( + std::optional & decision, const double distance, const PlannerParam & params); +/// @brief calculate the decision to slowdown or stop before an overlapping range +/// @param [in] range the range to check +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return an optional decision to slowdown or stop +std::optional calculate_decision( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger); +/// @brief calculate decisions to slowdown or stop before some overlapping ranges +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return the calculated decisions to slowdown or stop +std::vector calculate_decisions( + const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..31631fb577b09 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,150 @@ +// Copyright 2024 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 "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + if (predicted_path.path.empty() || stop_line.size() < 2) return; + + auto stop_line_idx = 0UL; + bool found = false; + lanelet::BasicSegment2d path_segment; + path_segment.first.x() = predicted_path.path.front().position.x; + path_segment.first.y() = predicted_path.path.front().position.y; + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; + if (boost::geometry::intersects(stop_line, path_segment)) { + found = true; + break; + } + path_segment.first = path_segment.second; + } + if (found) { + auto cut_idx = stop_line_idx; + double arc_length = 0; + while (cut_idx > 0 && arc_length < object_front_overhang) { + arc_length += tier4_autoware_utils::calcDistance2d( + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); + --cut_idx; + } + predicted_path.path.resize(cut_idx); + } +} + +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr planner_data) +{ + lanelet::ConstLanelets lanelets; + lanelet::BasicLineString2d query_line; + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); + const auto query_results = lanelet::geometry::findWithin2d( + planner_data->route_handler->getLaneletMapPtr()->laneletLayer, query_line); + for (const auto & r : query_results) lanelets.push_back(r.second); + for (const auto & ll : lanelets) { + for (const auto & element : ll.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->get_traffic_signal(element->id()); + if ( + traffic_signal_stamped.has_value() && element->stopLine().has_value() && + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { + lanelet::BasicLineString2d stop_line; + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); + return stop_line; + } + } + } + return std::nullopt; +} + +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr planner_data, const double object_front_overhang) +{ + const auto stop_line = find_next_stop_line(predicted_path, planner_data); + if (stop_line) { + // we use a longer stop line to also cut predicted paths that slightly go around the stop line + auto longer_stop_line = *stop_line; + const auto diff = stop_line->back() - stop_line->front(); + longer_stop_line.front() -= diff * 0.5; + longer_stop_line.back() += diff * 0.5; + cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang); + } +} + +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = planner_data->predicted_objects->header; + for (const auto & object : planner_data->predicted_objects->objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + if (no_overlap_path.size() <= 1) return true; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, planner_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); + } + + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..0770fbc0dcff1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,60 @@ +// Copyright 2024-2024 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 FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief cut a predicted path beyond the given stop line +/// @param [inout] predicted_path predicted path to cut +/// @param [in] stop_line stop line used for cutting +/// @param [in] object_front_overhang extra distance to cut ahead of the stop line +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); + +/// @brief find the next red light stop line along the given path +/// @param [in] path predicted path to check for a stop line +/// @param [in] planner_data planner data with stop line information +/// @return the first red light stop line found along the path (if any) +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr planner_data); + +/// @brief cut predicted path beyond stop lines of red lights +/// @param [inout] predicted_path predicted path to cut +/// @param [in] planner_data planner data to get the map and traffic light information +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr planner_data, const double object_front_overhang); + +/// @brief filter predicted objects and their predicted paths +/// @param [in] planner_data planner data +/// @param [in] ego_data ego data +/// @param [in] params parameters +/// @return filtered predicted objects +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp new file mode 100644 index 0000000000000..824a847b17414 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 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 "footprint.hpp" + +#include + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +tier4_autoware_utils::Polygon2d make_base_footprint( + const PlannerParam & p, const bool ignore_offset) +{ + tier4_autoware_utils::Polygon2d base_footprint; + const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; + const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; + const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; + const auto left_offset = ignore_offset ? 0.0 : p.extra_left_offset; + base_footprint.outer() = { + {p.front_offset + front_offset, p.left_offset + left_offset}, + {p.front_offset + front_offset, p.right_offset - right_offset}, + {p.rear_offset - rear_offset, p.right_offset - right_offset}, + {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + return base_footprint; +} + +lanelet::BasicPolygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +std::vector calculate_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params) +{ + const auto base_footprint = make_base_footprint(params); + std::vector trajectory_footprints; + trajectory_footprints.reserve(ego_data.trajectory_points.size()); + double length = 0.0; + const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + + params.front_offset + params.extra_front_offset; + for (auto i = ego_data.first_trajectory_idx; + i < ego_data.trajectory_points.size() && length < range; ++i) { + const auto & trajectory_pose = ego_data.trajectory_points[i].pose; + const auto angle = tf2::getYaw(trajectory_pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back( + p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); + trajectory_footprints.push_back(footprint); + if (i + 1 < ego_data.trajectory_points.size()) + length += tier4_autoware_utils::calcDistance2d( + trajectory_pose, ego_data.trajectory_points[i + 1].pose); + } + return trajectory_footprints; +} + +lanelet::BasicPolygon2d calculate_current_ego_footprint( + const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset) +{ + const auto base_footprint = make_base_footprint(params, ignore_offset); + const auto angle = tf2::getYaw(ego_data.pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); + return footprint; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp new file mode 100644 index 0000000000000..7b80dd9618bb7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 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 FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_velocity_planner +{ +namespace out_of_lane +{ +/// @brief create the base footprint of ego +/// @param [in] p parameters used to create the footprint +/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the +/// footprint +/// @return base ego footprint +tier4_autoware_utils::Polygon2d make_base_footprint( + const PlannerParam & p, const bool ignore_offset = false); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +lanelet::BasicPolygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief calculate the trajectory footprints +/// @details the resulting polygon follows the format used by the lanelet library: clockwise order +/// and implicit closing edge +/// @param [in] ego_data data related to the ego vehicle (includes its trajectory) +/// @param [in] params parameters +/// @return polygon footprints for each trajectory point starting from ego's current position +std::vector calculate_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params); +/// @brief calculate the current ego footprint +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] params parameters +/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the +/// footprint +lanelet::BasicPolygon2d calculate_current_ego_footprint( + const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); +} // namespace out_of_lane +} // namespace autoware::motion_velocity_planner + +#endif // FOOTPRINT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp new file mode 100644 index 0000000000000..23dbf77f08151 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -0,0 +1,126 @@ +// Copyright 2024 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 "lanelets_selection.hpp" + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +lanelet::ConstLanelets consecutive_lanelets( + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + consecutives.insert(consecutives.end(), previous.begin(), previous.end()); + return consecutives; +} + +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler) +{ + lanelet::ConstLanelets missing_lane_change_lanelets; + const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + lanelet::ConstLanelets adjacents; + lanelet::ConstLanelets consecutives; + for (const auto & ll : trajectory_lanelets) { + const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); + std::copy_if( + consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), + [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); + const auto adjacents_of_ll = routing_graph.besides(ll); + std::copy_if( + adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), + [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); + } + std::copy_if( + adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), + [&](const auto & l) { + return !contains_lanelet(missing_lane_change_lanelets, l.id()) && + !contains_lanelet(trajectory_lanelets, l.id()) && + contains_lanelet(consecutives, l.id()); + }); + return missing_lane_change_lanelets; +} + +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets trajectory_lanelets; + lanelet::BasicLineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); + for (const auto & dist_lanelet : + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, trajectory_ls)) { + if (!contains_lanelet(trajectory_lanelets, dist_lanelet.second.id())) + trajectory_lanelets.push_back(dist_lanelet.second); + } + const auto missing_lanelets = + get_missing_lane_change_lanelets(trajectory_lanelets, route_handler); + trajectory_lanelets.insert( + trajectory_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); + return trajectory_lanelets; +} + +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets ignored_lanelets; + // ignore lanelets directly behind ego + const auto behind = + tier4_autoware_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); + const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); + const auto behind_lanelets = lanelet::geometry::findWithin2d( + route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); + for (const auto & l : behind_lanelets) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + } + return ignored_lanelets; +} + +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets other_lanelets; + const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); + const auto lanelets_within_range = lanelet::geometry::findWithin2d( + route_handler->getLaneletMapPtr()->laneletLayer, ego_point, + std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + + params.extra_front_offset); + for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); + const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); + if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + } + return other_lanelets; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp new file mode 100644 index 0000000000000..c7351f0a98e89 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 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 LANELETS_SELECTION_HPP_ +#define LANELETS_SELECTION_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief checks if a lanelet is already contained in a vector of lanelets +/// @param [in] lanelets vector to check +/// @param [in] id lanelet id to check +/// @return true if the given vector contains a lanelet of the given id +inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lanelet::Id id) +{ + return std::find_if(lanelets.begin(), lanelets.end(), [&](const auto & l) { + return l.id() == id; + }) != lanelets.end(); +}; + +/// @brief calculate lanelets crossed by the ego trajectory +/// @details calculated from the ids of the trajectory msg, the lanelets containing trajectory +/// points +/// @param [in] ego_data data about the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets crossed by the ego vehicle +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler); +/// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during +/// a lane change +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets that may be overlapped by a lane change (and are not already in +/// trajectory_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler); +/// @brief calculate lanelets that should be ignored +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @param [in] params parameters +/// @return lanelets to ignore +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +/// @brief calculate lanelets that should be checked by the module +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] ignored_lanelets lanelets to ignore +/// @param [in] route_handler route handler +/// @param [in] params parameters +/// @return lanelets to check for overlaps +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp new file mode 100644 index 0000000000000..b662186049342 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -0,0 +1,305 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// 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 "out_of_lane_module.hpp" + +#include "calculate_slowdown_points.hpp" +#include "debug.hpp" +#include "decisions.hpp" +#include "filter_predicted_objects.hpp" +#include "footprint.hpp" +#include "lanelets_selection.hpp" +#include "overlapping_range.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + logger_ = node.get_logger(); + clock_ = node.get_clock(); + init_parameters(node); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + debug_publisher_ = + node.create_publisher("~/" + ns_ + "/debug_markers", 1); + virtual_wall_publisher_ = + node.create_publisher("~/" + ns_ + "/virtual_walls", 1); +} +void OutOfLaneModule::init_parameters(rclcpp::Node & node) +{ + using tier4_autoware_utils::getOrDeclareParameter; + auto & pp = params_; + + pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); + pp.skip_if_already_overlapping = + getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + + pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); + pp.intervals_ego_buffer = getOrDeclareParameter(node, ns_ + ".intervals.ego_time_buffer"); + pp.intervals_obj_buffer = + getOrDeclareParameter(node, ns_ + ".intervals.objects_time_buffer"); + pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); + + pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); + pp.objects_use_predicted_paths = + getOrDeclareParameter(node, ns_ + ".objects.use_predicted_paths"); + pp.objects_min_confidence = + getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); + pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); + pp.objects_cut_predicted_paths_beyond_red_lights = + getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); + + pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); + pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); + + pp.skip_if_over_max_decel = + getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); + pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); + pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); + pp.dist_buffer = getOrDeclareParameter(node, ns_ + ".action.distance_buffer"); + pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); + pp.slow_dist_threshold = + getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); + pp.stop_dist_threshold = + getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); + + pp.ego_min_velocity = getOrDeclareParameter(node, ns_ + ".ego.min_assumed_velocity"); + pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); + pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); + pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); + pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.front_offset = vehicle_info.max_longitudinal_offset_m; + pp.rear_offset = vehicle_info.min_longitudinal_offset_m; + pp.left_offset = vehicle_info.max_lateral_offset_m; + pp.right_offset = vehicle_info.min_lateral_offset_m; +} + +void OutOfLaneModule::update_parameters(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + auto & pp = params_; + updateParam(parameters, ns_ + ".mode", pp.mode); + updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + + updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); + updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); + updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); + updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); + + updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); + updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); + updateParam( + parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); + updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); + updateParam( + parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", + pp.objects_cut_predicted_paths_beyond_red_lights); + + updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); + updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); + + updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); + updateParam(parameters, ns_ + ".action.precision", pp.precision); + updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); + updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); + updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); + updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); + + updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); + updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); + updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); + updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); + updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); +} + +VelocityPlanningResult OutOfLaneModule::plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + VelocityPlanningResult result; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + out_of_lane::EgoData ego_data; + ego_data.pose = planner_data->current_odometry->pose; + ego_data.trajectory_points = ego_trajectory_points; + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.velocity = planner_data->current_velocity->twist.linear.x; + ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); + stopwatch.tic("calculate_trajectory_footprints"); + const auto current_ego_footprint = + out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); + const auto trajectory_footprints = + out_of_lane::calculate_trajectory_footprints(ego_data, params_); + const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); + // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); + const auto trajectory_lanelets = + out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); + const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( + ego_data, trajectory_lanelets, planner_data->route_handler, params_); + const auto other_lanelets = out_of_lane::calculate_other_lanelets( + ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); + + debug_data_.reset_data(); + debug_data_.trajectory_points = ego_trajectory_points; + debug_data_.footprints = trajectory_footprints; + debug_data_.trajectory_lanelets = trajectory_lanelets; + debug_data_.ignored_lanelets = ignored_lanelets; + debug_data_.other_lanelets = other_lanelets; + debug_data_.first_trajectory_idx = ego_data.first_trajectory_idx; + + if (params_.skip_if_already_overlapping) { + debug_data_.current_footprint = current_ego_footprint; + const auto overlapped_lanelet_it = + std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { + return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); + }); + if (overlapped_lanelet_it != other_lanelets.end()) { + debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); + RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); + return result; + } + } + // Calculate overlapping ranges + stopwatch.tic("calculate_overlapping_ranges"); + const auto ranges = out_of_lane::calculate_overlapping_ranges( + trajectory_footprints, trajectory_lanelets, other_lanelets, params_); + const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); + // Calculate stop and slowdown points + out_of_lane::DecisionInputs inputs; + inputs.ranges = ranges; + inputs.ego_data = ego_data; + stopwatch.tic("filter_predicted_objects"); + inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); + inputs.route_handler = planner_data->route_handler; + inputs.lanelets = other_lanelets; + stopwatch.tic("calculate_decisions"); + const auto decisions = out_of_lane::calculate_decisions(inputs, params_, logger_); + const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); + stopwatch.tic("calc_slowdown_points"); + if ( // reset the previous inserted point if the timer expired + prev_inserted_point_ && + (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) + prev_inserted_point_.reset(); + auto point_to_insert = + out_of_lane::calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); + const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); + stopwatch.tic("insert_slowdown_points"); + debug_data_.slowdowns.clear(); + if ( // reset the timer if there is no previous inserted point or if we avoid the same lane + point_to_insert && + (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == + point_to_insert->slowdown.lane_to_avoid.id())) + prev_inserted_point_time_ = clock_->now(); + // reuse previous stop point if there is no new one or if its velocity is not higher than the new + // one and its arc length is lower + const auto should_use_prev_inserted_point = [&]() { + if ( + point_to_insert && prev_inserted_point_ && + prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { + const auto arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, point_to_insert->point.pose.position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + return prev_arc_length < arc_length; + } + return !point_to_insert && prev_inserted_point_; + }(); + if (should_use_prev_inserted_point) { + // if the trajectory changed the prev point is no longer on the trajectory so we project it + const auto insert_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + prev_inserted_point_->point.pose = + motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); + point_to_insert = prev_inserted_point_; + } + if (point_to_insert) { + prev_inserted_point_ = point_to_insert; + RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); + debug_data_.slowdowns = {*point_to_insert}; + if (point_to_insert->slowdown.velocity == 0.0) + result.stop_points.push_back(point_to_insert->point.pose.position); + else + result.slowdown_intervals.emplace_back( + point_to_insert->point.pose.position, point_to_insert->point.pose.position, + point_to_insert->slowdown.velocity); + + const auto is_approaching = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, + point_to_insert->point.pose.position) > 0.1 && + ego_data.velocity > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; + velocity_factor_interface_.set( + ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + result.velocity_factor = velocity_factor_interface_.get(); + } else if (!decisions.empty()) { + RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); + } + const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); + debug_data_.ranges = inputs.ranges; + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n" + "\tcalculate_lanelets = %2.0fus\n" + "\tcalculate_trajectory_footprints = %2.0fus\n" + "\tcalculate_overlapping_ranges = %2.0fus\n" + "\tfilter_pred_objects = %2.0fus\n" + "\tcalculate_decisions = %2.0fus\n" + "\tcalc_slowdown_points = %2.0fus\n" + "\tinsert_slowdown_points = %2.0fus\n", + total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(debug_data_, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + return result; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::OutOfLaneModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp new file mode 100644 index 0000000000000..8a20d5c850a09 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 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 OUT_OF_LANE_MODULE_HPP_ +#define OUT_OF_LANE_MODULE_HPP_ + +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class OutOfLaneModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override { return module_name_; } + +private: + void init_parameters(rclcpp::Node & node); + out_of_lane::PlannerParam params_; + + inline static const std::string ns_ = "out_of_lane"; + std::string module_name_; + std::optional prev_inserted_point_{}; + rclcpp::Clock::SharedPtr clock_{}; + rclcpp::Time prev_inserted_point_time_{}; + +protected: + // Debug + mutable out_of_lane::DebugData debug_data_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OUT_OF_LANE_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp new file mode 100644 index 0000000000000..f89c620b75fb6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -0,0 +1,127 @@ +// Copyright 2024 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 "overlapping_range.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet) +{ + Overlap overlap; + const auto & left_bound = lanelet.leftBound2d().basicLineString(); + const auto & right_bound = lanelet.rightBound2d().basicLineString(); + // TODO(Maxime): these intersects (for each trajectory footprint, for each lanelet) are too + // expensive + const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); + const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); + + lanelet::BasicPolygons2d overlapping_polygons; + if (overlap_left || overlap_right) + boost::geometry::intersection( + trajectory_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); + for (const auto & overlapping_polygon : overlapping_polygons) { + for (const auto & point : overlapping_polygon) { + if (overlap_left && overlap_right) + overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); + else if (overlap_left) + overlap.inside_distance = + std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); + else if (overlap_right) + overlap.inside_distance = + std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); + geometry_msgs::msg::Pose p; + p.position.x = point.x(); + p.position.y = point.y(); + const auto length = lanelet::utils::getArcCoordinates(trajectory_lanelets, p).length; + if (length > overlap.max_arc_length) { + overlap.max_arc_length = length; + overlap.max_overlap_point = point; + } + if (length < overlap.min_arc_length) { + overlap.min_arc_length = length; + overlap.min_overlap_point = point; + } + } + } + return overlap; +} + +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params) +{ + OverlapRanges ranges; + OtherLane other_lane(lanelet); + std::vector overlaps; + for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { + const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_lanelets, lanelet); + const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; + if (has_overlap) { // open/update the range + overlaps.push_back(overlap); + if (!other_lane.range_is_open) { + other_lane.first_range_bound.index = i; + other_lane.first_range_bound.point = overlap.min_overlap_point; + other_lane.first_range_bound.arc_length = + overlap.min_arc_length - params.overlap_extra_length; + other_lane.first_range_bound.inside_distance = overlap.inside_distance; + other_lane.range_is_open = true; + } + other_lane.last_range_bound.index = i; + other_lane.last_range_bound.point = overlap.max_overlap_point; + other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; + other_lane.last_range_bound.inside_distance = overlap.inside_distance; + } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } + } + // close the range if it is still open + if (other_lane.range_is_open) { + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } + return ranges; +} + +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params) +{ + OverlapRanges ranges; + for (auto & lanelet : lanelets) { + const auto lanelet_ranges = + calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); + ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); + } + return ranges; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp new file mode 100644 index 0000000000000..07c8663ea21bc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 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 OVERLAPPING_RANGE_HPP_ +#define OVERLAPPING_RANGE_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the overlap between the given footprint and lanelet +/// @param [in] path_footprint footprint used to calculate the overlap +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelet lanelet used to calculate the overlap +/// @return the found overlap between the footprint and the lanelet +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); +/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelet lanelet used to calculate the overlaps +/// @param [in] params parameters +/// @return the overlapping ranges found between the footprints and the lanelet +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params); +/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelets lanelets used to calculate the overlaps +/// @param [in] params parameters +/// @return the overlapping ranges found between the footprints and the lanelets, sorted by +/// increasing arc length along the trajectory +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp new file mode 100644 index 0000000000000..04f9f1ccac4c2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -0,0 +1,236 @@ +// Copyright 2024 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + std::string mode; // mode used to consider a conflict with an object + bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps + // another lane + + double time_threshold; // [s](mode="threshold") objects time threshold + double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range + double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range + double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range + + bool objects_use_predicted_paths; // whether to use the objects' predicted paths + bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red + // lights' stop lines + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path + double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in + // the other lane + + double overlap_extra_length; // [m] extra length to add around an overlap range + double overlap_min_dist; // [m] min distance inside another lane to consider an overlap + // action to insert in the trajectory if an object causes a conflict at an overlap + bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel + double dist_buffer; + double slow_velocity; + double slow_dist_threshold; + double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the trajectory + double min_decision_duration; // [s] minimum duration needed a decision can be canceled + // ego dimensions used to create its polygon footprint + double front_offset; // [m] front offset (from vehicle info) + double rear_offset; // [m] rear offset (from vehicle info) + double right_offset; // [m] right offset (from vehicle info) + double left_offset; // [m] left offset (from vehicle info) + double extra_front_offset; // [m] extra front distance + double extra_rear_offset; // [m] extra rear distance + double extra_right_offset; // [m] extra right distance + double extra_left_offset; // [m] extra left distance +}; + +struct EnterExitTimes +{ + double enter_time{}; + double exit_time{}; +}; +struct RangeTimes +{ + EnterExitTimes ego{}; + EnterExitTimes object{}; +}; + +/// @brief action taken by the "out of lane" module +struct Slowdown +{ + size_t target_trajectory_idx{}; // we want to slowdown before this trajectory index + double velocity{}; // desired slow down velocity + lanelet::ConstLanelet lane_to_avoid{}; // we want to slowdown before entering this lane +}; +/// @brief slowdown to insert in a trajectory +struct SlowdownToInsert +{ + Slowdown slowdown{}; + autoware_auto_planning_msgs::msg::TrajectoryPoint point{}; +}; + +/// @brief bound of an overlap range (either the first, or last bound) +struct RangeBound +{ + size_t index{}; + lanelet::BasicPoint2d point{}; + double arc_length{}; + double inside_distance{}; +}; + +/// @brief representation of an overlap between the ego footprint and some other lane +struct Overlap +{ + double inside_distance = 0.0; ///!< distance inside the overlap + double min_arc_length = std::numeric_limits::infinity(); + double max_arc_length = 0.0; + lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length + lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length +}; + +/// @brief range along the trajectory where ego overlaps another lane +struct OverlapRange +{ + lanelet::ConstLanelet lane{}; + size_t entering_trajectory_idx{}; + size_t exiting_trajectory_idx{}; + lanelet::BasicPoint2d entering_point{}; // pose of the overlapping point closest along the lane + lanelet::BasicPoint2d exiting_point{}; // pose of the overlapping point furthest along the lane + double inside_distance{}; // [m] how much ego footprint enters the lane + mutable struct + { + std::vector overlaps{}; + std::optional decision{}; + RangeTimes times{}; + std::optional object{}; + } debug; +}; +using OverlapRanges = std::vector; +/// @brief representation of a lane and its current overlap range +struct OtherLane +{ + bool range_is_open = false; + RangeBound first_range_bound{}; + RangeBound last_range_bound{}; + lanelet::ConstLanelet lanelet{}; + lanelet::BasicPolygon2d polygon{}; + + explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) + { + polygon = lanelet.polygon2d().basicPolygon(); + } + + [[nodiscard]] OverlapRange close_range() + { + OverlapRange range; + range.lane = lanelet; + range.entering_trajectory_idx = first_range_bound.index; + range.entering_point = first_range_bound.point; + range.exiting_trajectory_idx = last_range_bound.index; + range.exiting_point = last_range_bound.point; + range.inside_distance = + std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); + range_is_open = false; + last_range_bound = {}; + return range; + } +}; + +/// @brief data related to the ego vehicle +struct EgoData +{ + std::vector trajectory_points{}; + size_t first_trajectory_idx{}; + double velocity{}; // [m/s] + double max_decel{}; // [m/s²] + geometry_msgs::msg::Pose pose{}; +}; + +/// @brief data needed to make decisions +struct DecisionInputs +{ + OverlapRanges ranges{}; + EgoData ego_data{}; + autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + std::shared_ptr route_handler{}; + lanelet::ConstLanelets lanelets{}; +}; + +/// @brief debug data +struct DebugData +{ + std::vector footprints; + std::vector slowdowns; + geometry_msgs::msg::Pose ego_pose; + OverlapRanges ranges; + lanelet::BasicPolygon2d current_footprint; + lanelet::ConstLanelets current_overlapped_lanelets; + lanelet::ConstLanelets trajectory_lanelets; + lanelet::ConstLanelets ignored_lanelets; + lanelet::ConstLanelets other_lanelets; + std::vector trajectory_points; + size_t first_trajectory_idx; + + size_t prev_footprints = 0; + size_t prev_slowdowns = 0; + size_t prev_ranges = 0; + size_t prev_current_overlapped_lanelets = 0; + size_t prev_ignored_lanelets = 0; + size_t prev_trajectory_lanelets = 0; + size_t prev_other_lanelets = 0; + void reset_data() + { + prev_footprints = footprints.size(); + footprints.clear(); + prev_slowdowns = slowdowns.size(); + slowdowns.clear(); + prev_ranges = ranges.size(); + ranges.clear(); + prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); + current_overlapped_lanelets.clear(); + prev_ignored_lanelets = ignored_lanelets.size(); + ignored_lanelets.clear(); + prev_trajectory_lanelets = trajectory_lanelets.size(); + trajectory_lanelets.clear(); + prev_other_lanelets = other_lanelets.size(); + other_lanelets.clear(); + } +}; + +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp new file mode 100644 index 0000000000000..4961cd064efaf --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -0,0 +1,58 @@ +// Copyright 2024 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 "../src/filter_predicted_objects.hpp" + +#include +#include +#include + +#include +#include + +TEST(TestCollisionDistance, CutPredictedPathBeyondLine) +{ + using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + lanelet::BasicLineString2d stop_line; + double object_front_overhang = 0.0; + const auto eps = 1e-9; + + EXPECT_NO_THROW(cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang)); + + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + predicted_path.path.push_back(pose); + pose.position.y = 1.0; + predicted_path.path.push_back(pose); + pose.position.y = 2.0; + predicted_path.path.push_back(pose); + pose.position.y = 3.0; + predicted_path.path.push_back(pose); + + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 4UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } + stop_line = {{-1.0, 2.0}, {1.0, 2.0}}; + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 2UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..f1eb7ff047fdc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +# ament_auto_add_library(${PROJECT_NAME}_lib SHARED +# DIRECTORY src +# ) + +ament_auto_package(INSTALL_TO_SHARE + include +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp new file mode 100644 index 0000000000000..84591f9429a66 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -0,0 +1,106 @@ +// Copyright 2024 Autoware Foundation +// +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficSignal signal; +}; +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node) + : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + { + } + + // msgs from callbacks that are used for data-ready + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; + autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + std::shared_ptr route_handler; + + // nearest search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + // other internal data + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; + std::optional external_velocity_limit; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + + // velocity smoother + std::shared_ptr velocity_smoother_{}; + // parameters + vehicle_info_util::VehicleInfo vehicle_info_; + + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional get_traffic_signal( + const lanelet::Id id, const bool keep_last_observation = false) const + { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + return std::make_optional(traffic_light_id_map.at(id)); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp new file mode 100644 index 0000000000000..9011d3fb7e079 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ + +#include "planner_data.hpp" +#include "velocity_planning_result.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +class PluginModuleInterface +{ +public: + virtual ~PluginModuleInterface() = default; + virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; + virtual void update_parameters(const std::vector & parameters) = 0; + virtual VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) = 0; + virtual std::string get_module_name() const = 0; + motion_utils::VelocityFactorInterface velocity_factor_interface_; + rclcpp::Logger logger_ = rclcpp::get_logger(""); + rclcpp::Publisher::SharedPtr debug_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp new file mode 100644 index 0000000000000..9b011b74503f2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Autoware Foundation +// +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct SlowdownInterval +{ + SlowdownInterval( + const geometry_msgs::msg::Point & from_, const geometry_msgs::msg::Point & to_, + const double vel) + : from{from_}, to{to_}, velocity{vel} + { + } + geometry_msgs::msg::Point from{}; + geometry_msgs::msg::Point to{}; + double velocity{}; +}; +struct VelocityPlanningResult +{ + std::vector stop_points{}; + std::vector slowdown_intervals{}; + std::optional velocity_factor{}; +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml new file mode 100644 index 0000000000000..ad791d247ee11 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_planner_common + 0.1.0 + Common functions and interfaces for motion_velocity_planner modules + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + motion_velocity_smoother + rclcpp + route_handler + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt new file mode 100644 index 0000000000000..0af4da6fd4426 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_node) + +find_package(autoware_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "srv/LoadPlugin.srv" + "srv/UnloadPlugin.srv" + DEPENDENCIES +) + +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "autoware::motion_velocity_planner::MotionVelocityPlannerNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_lib + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") +endif() + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md new file mode 100644 index 0000000000000..f8c5eb172abd2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -0,0 +1,58 @@ +# Motion Velocity Planner + +## Overview + +`motion_velocity_planner` is a planner to adjust the trajectory velocity based on the obstacles around the vehicle. +It loads modules as plugins. Please refer to the links listed below for detail on each module. + +![Architecture](./docs/MotionVelocityPlanner-InternalInterface.drawio.svg) + +- [Out of Lane](../autoware_motion_velocity_out_of_lane_module/README.md) + +Each module calculates stop and slow down points to be inserted in the ego trajectory. +These points are assumed to correspond to the `base_link` frame of the ego vehicle as it follows the trajectory. +This means that to stop before a wall, a stop point is inserted in the trajectory at a distance ahead of the wall equal to the vehicle front offset (wheelbase + front overhang, see the [vehicle dimensions](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/). + +![set_stop_velocity](./docs/set_stop_velocity.drawio.svg) + +## Input topics + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | +| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | + +## Output topics + +| Name | Type | Description | +| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | + +## Services + +| Name | Type | Description | +| ------------------------- | -------------------------------------------------------- | ---------------------------- | +| `~/service/load_plugin` | autoware_motion_velocity_planner_node::srv::LoadPlugin | To request loading a plugin | +| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin | + +## Node parameters + +| Parameter | Type | Description | +| ---------------- | ---------------- | ---------------------- | +| `launch_modules` | vector\ | module names to launch | + +In addition, the following parameters should be provided to the node: + +- [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml); +- [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml); +- [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml); +- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/motion_velocity_smoother/#parameters) +- Parameters of each plugin that will be loaded. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml new file mode 100644 index 0000000000000..5b2fea537d4f7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg new file mode 100644 index 0000000000000..ea3b7ad99d4e5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg @@ -0,0 +1,538 @@ + + + + + + + + + + + + + + + +
+
+
+ ~/input/dynamic_objects +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/vehicle_odometry +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/trajectory +
+
+
+ + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + Motion Velocity Planner + + + + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + +
+
+
+ + Updated +
+ Trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+
Stop/Slowdown
+
+ points +
+
+
+
+
+ + + + + + + + + + + + + +
+
+
+ Data +
+
+
+ + + + + + + + + + + + +
+
+
+ Planner Data +
+
+
+ + + + + + + + + + + + + Loaded Modules + + + + + + + + + + out_of_lane + + + + + + + + + + obstacle_velocity_limiter + + + + + + + + + + dynamic_obstacle_stop + + + + + + + + + +
+
+
...
+
+
+ + + + + + + + + + + + +
+
+
+ Smooth Velocity +
+
+
+ + + + + + + + + + + + + obstacle_stop + + + + + + + + + + +
+
+
+ ~/input/vector_map +
+
+
+ + + + + + + + + + + + +
+
+
+ + ~/output/trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+ ... +
+
+
+ + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg new file mode 100644 index 0000000000000..5ae7d7dbb5461 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg @@ -0,0 +1,197 @@ + + + + + + + + + + + +
+
+
+ +

+ + stop point + +

+ +
+
+
+ + stop point + + + + + + + + + + + +
+
+
+ +

+ + v + +

+ +
+
+
+ + v + + + + + + +
+
+
+ +

+ + x + +

+ +
+
+
+ + x + + + + + + +
+
+
+ +

+ + baselink to front + +

+ +
+
+
+ + baselink to front + + + + + + + + + + + + + + +
+
+
+ +

+ + output velocity + +

+ +
+
+
+ + output velocity + + + + + + + +
+
+
+ +

+ + reference velocity + +

+ +
+
+
+ + reference velocity + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml new file mode 100644 index 0000000000000..3732d86ef380a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml new file mode 100644 index 0000000000000..2b52610c0276a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -0,0 +1,57 @@ + + + + autoware_motion_velocity_planner_node + 0.1.0 + Node of the motion_velocity_planner + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + rosidl_default_generators + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_motion_velocity_planner_common + diagnostic_msgs + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + motion_velocity_smoother + pcl_conversions + pluginlib + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + rosidl_default_runtime + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_motion_velocity_out_of_lane_module + autoware_planning_test_manager + + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json new file mode 100644 index 0000000000000..7db22e5e39d17 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for the Motion Velocity Planner", + "type": "object", + "definitions": { + "motion_velocity_planner": { + "type": "object", + "properties": { + "smooth_velocity_before_planning": { + "type": "boolean", + "default": true, + "description": "if true, smooth the velocity profile of the input trajectory before planning" + } + }, + "required": ["smooth_velocity_before_planning"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/motion_velocity_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp new file mode 100644 index 0000000000000..c0373678270dd --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -0,0 +1,458 @@ +// Copyright 2024 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 "node.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace +{ +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} +} // namespace + +namespace autoware::motion_velocity_planner +{ +MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("motion_velocity_planner_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + planner_data_(*this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Subscribers + sub_trajectory_ = this->create_subscription( + "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), + create_subscription_options(this)); + sub_predicted_objects_ = + this->create_subscription( + "~/input/dynamic_objects", 1, + std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), + create_subscription_options(this)); + sub_no_ground_pointcloud_ = this->create_subscription( + "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1), + create_subscription_options(this)); + sub_vehicle_odometry_ = this->create_subscription( + "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1), + create_subscription_options(this)); + sub_acceleration_ = this->create_subscription( + "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), + create_subscription_options(this)); + sub_lanelet_map_ = this->create_subscription( + "~/input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), + create_subscription_options(this)); + sub_traffic_signals_ = + this->create_subscription( + "~/input/traffic_signals", 1, + std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), + create_subscription_options(this)); + sub_virtual_traffic_light_states_ = + this->create_subscription( + "~/input/virtual_traffic_light_states", 1, + std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), + create_subscription_options(this)); + sub_occupancy_grid_ = this->create_subscription( + "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1), + create_subscription_options(this)); + + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&MotionVelocityPlannerNode::on_unload_plugin, this, _1, _2)); + + // Publishers + trajectory_pub_ = + this->create_publisher("~/output/trajectory", 1); + velocity_factor_publisher_ = + this->create_publisher( + "~/output/velocity_factors", 1); + + // Parameters + smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); + // nearest search + planner_data_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + // set velocity smoother param + set_velocity_smoother_params(); + + // Initialize PlannerManager + for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } + planner_manager_.load_module_plugin(*this, name); + } + + set_param_callback_ = this->add_on_set_parameters_callback( + std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); +} + +void MotionVelocityPlannerNode::on_load_plugin( + const LoadPlugin::Request::SharedPtr request, + [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.load_module_plugin(*this, request->plugin_name); +} + +void MotionVelocityPlannerNode::on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.unload_module_plugin(*this, request->plugin_name); +} + +// NOTE: argument planner_data must not be referenced for multithreading +bool MotionVelocityPlannerNode::is_data_ready() const +{ + const auto & d = planner_data_; + auto clock = *get_clock(); + const auto check_with_msg = [&](const auto ptr, const auto & msg) { + constexpr auto throttle_duration = 3000; // [ms] + if (!ptr) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg); + return false; + } + return true; + }; + + return check_with_msg(d.current_odometry, "Waiting for current odometry") && + check_with_msg(d.current_velocity, "Waiting for current velocity") && + check_with_msg(d.current_acceleration, "Waiting for current acceleration") && + check_with_msg(d.predicted_objects, "Waiting for predicted objects") && + check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") && + check_with_msg(map_ptr_, "Waiting for the map") && + check_with_msg( + d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") && + check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid"); +} + +void MotionVelocityPlannerNode::on_occupancy_grid( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.occupancy_grid = msg; +} + +void MotionVelocityPlannerNode::on_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.predicted_objects = msg; +} + +void MotionVelocityPlannerNode::on_no_ground_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); + return; + } + + pcl::PointCloud pc; + pcl::fromROSMsg(*msg, pc); + + Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); + pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); + if (!pc.empty()) { + tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + } + + { + std::lock_guard lock(mutex_); + planner_data_.no_ground_pointcloud = pc_transformed; + } +} + +void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + auto current_odometry = std::make_shared(); + current_odometry->header = msg->header; + current_odometry->pose = msg->pose.pose; + planner_data_.current_odometry = current_odometry; + + auto current_velocity = std::make_shared(); + current_velocity->header = msg->header; + current_velocity->twist = msg->twist.twist; + planner_data_.current_velocity = current_velocity; +} + +void MotionVelocityPlannerNode::on_acceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.current_acceleration = msg; +} + +void MotionVelocityPlannerNode::set_velocity_smoother_params() +{ + planner_data_.velocity_smoother_ = + std::make_shared(*this); +} + +void MotionVelocityPlannerNode::on_lanelet_map( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + map_ptr_ = msg; + has_received_map_ = true; +} + +void MotionVelocityPlannerNode::on_traffic_signals( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); + for (const auto & signal : msg->signals) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } + } +} + +void MotionVelocityPlannerNode::on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.virtual_traffic_light_states = msg; +} + +void MotionVelocityPlannerNode::on_trajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) +{ + std::unique_lock lk(mutex_); + + if (!is_data_ready()) { + return; + } + + if (input_trajectory_msg->points.empty()) { + RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); + return; + } + + if (has_received_map_) { + planner_data_.route_handler = std::make_shared(*map_ptr_); + has_received_map_ = false; + } + + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ + input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; + + auto output_trajectory_msg = generate_trajectory(input_trajectory_points); + output_trajectory_msg.header = input_trajectory_msg->header; + + lk.unlock(); + + trajectory_pub_->publish(output_trajectory_msg); + published_time_publisher_->publish_if_subscribed( + trajectory_pub_, output_trajectory_msg.header.stamp); +} + +void MotionVelocityPlannerNode::insert_stop( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const +{ + const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); + const auto insert_idx = motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); + if (insert_idx) { + for (auto idx = *insert_idx; idx < trajectory.points.size(); ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert stop point"); + } +} + +void MotionVelocityPlannerNode::insert_slowdown( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const +{ + const auto from_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); + const auto from_insert_idx = + motion_utils::insertTargetPoint(from_seg_idx, slowdown_interval.from, trajectory.points); + const auto to_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); + const auto to_insert_idx = + motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); + if (from_insert_idx && to_insert_idx) { + for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); + } +} + +autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const +{ + const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose; + const double v0 = planner_data.current_velocity->twist.linear.x; + const double a0 = planner_data.current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data.external_velocity_limit; + const auto & smoother = planner_data.velocity_smoother_; + + const auto traj_lateral_acc_filtered = + smoother->applyLateralAccelerationFilter(trajectory_points); + + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_steering_rate_limited, v0, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + autoware::motion_velocity_planner::TrajectoryPoints clipped; + autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + RCLCPP_ERROR(get_logger(), "failed to smooth"); + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } + return traj_smoothed; +} + +autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) +{ + autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg; + output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; + if (smooth_velocity_before_planning_) + input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); + + const auto planning_results = planner_manager_.plan_velocities( + input_trajectory_points, std::make_shared(planner_data_)); + + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; + velocity_factors.header.frame_id = "map"; + velocity_factors.header.stamp = get_clock()->now(); + + for (const auto & planning_result : planning_results) { + for (const auto & stop_point : planning_result.stop_points) + insert_stop(output_trajectory_msg, stop_point); + for (const auto & slowdown_interval : planning_result.slowdown_intervals) + insert_slowdown(output_trajectory_msg, slowdown_interval); + if (planning_result.velocity_factor) + velocity_factors.factors.push_back(*planning_result.velocity_factor); + } + + velocity_factor_publisher_->publish(velocity_factors); + return output_trajectory_msg; +} + +rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + { + std::unique_lock lk(mutex_); // for planner_manager_ + planner_manager_.update_module_parameters(parameters); + } + + updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); + updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + + set_velocity_smoother_params(); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} +} // namespace autoware::motion_velocity_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion_velocity_planner::MotionVelocityPlannerNode) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp new file mode 100644 index 0000000000000..ecf128bf9a012 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -0,0 +1,140 @@ +// Copyright 2024 Autoware Foundation +// +// 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 NODE_HPP_ +#define NODE_HPP_ + +#include "planner_manager.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_motion_velocity_planner_node::srv::LoadPlugin; +using autoware_motion_velocity_planner_node::srv::UnloadPlugin; +using TrajectoryPoints = std::vector; + +class MotionVelocityPlannerNode : public rclcpp::Node +{ +public: + explicit MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // subscriber + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_objects_; + rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; + rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + rclcpp::Subscription::SharedPtr sub_acceleration_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_virtual_traffic_light_states_; + rclcpp::Subscription::SharedPtr sub_occupancy_grid_; + + void on_trajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); + void on_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_traffic_signals( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); + void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + + // publishers + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr + velocity_factor_publisher_; + + // parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; + bool smooth_velocity_before_planning_{}; + /// @brief set parameters of the velocity smoother + void set_velocity_smoother_params(); + + // members + PlannerData planner_data_; + MotionVelocityPlannerManager planner_manager_; + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + bool has_received_map_ = false; + + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + const UnloadPlugin::Response::SharedPtr response); + void on_load_plugin( + const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + rcl_interfaces::msg::SetParametersResult on_set_param( + const std::vector & parameters); + + // mutex for planner_data_ + std::mutex mutex_; + + // function + bool is_data_ready() const; + void insert_stop( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const; + void insert_slowdown( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const; + autoware::motion_velocity_planner::TrajectoryPoints smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const; + autoware_auto_planning_msgs::msg::Trajectory generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // NODE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp new file mode 100644 index 0000000000000..fa3a3b1ae5dcb --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 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 "planner_manager.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ + +MotionVelocityPlannerManager::MotionVelocityPlannerManager() +: plugin_loader_( + "autoware_motion_velocity_planner_node", + "autoware::motion_velocity_planner::PluginModuleInterface") +{ +} + +void MotionVelocityPlannerManager::load_module_plugin(rclcpp::Node & node, const std::string & name) +{ + // Check if the plugin is already loaded. + if (plugin_loader_.isClassLoaded(name)) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node, name); + + // register + loaded_plugins_.push_back(plugin); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } +} + +void MotionVelocityPlannerManager::unload_module_plugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if(loaded_plugins_.begin(), loaded_plugins_.end(), [&](const auto plugin) { + return plugin->get_module_name() == name; + }); + + if (it == loaded_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + loaded_plugins_.erase(it, loaded_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + +void MotionVelocityPlannerManager::update_module_parameters( + const std::vector & parameters) +{ + for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); +} + +std::vector MotionVelocityPlannerManager::plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + std::vector results; + for (auto & plugin : loaded_plugins_) + results.push_back(plugin->plan(ego_trajectory_points, planner_data)); + return results; +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp new file mode 100644 index 0000000000000..a5e330535fc73 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Autoware Foundation +// +// 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 PLANNER_MANAGER_HPP_ +#define PLANNER_MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MotionVelocityPlannerManager +{ +public: + MotionVelocityPlannerManager(); + void load_module_plugin(rclcpp::Node & node, const std::string & name); + void unload_module_plugin(rclcpp::Node & node, const std::string & name); + void update_module_parameters(const std::vector & parameters); + std::vector plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data); + +private: + pluginlib::ClassLoader plugin_loader_; + std::vector> loaded_plugins_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PLANNER_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp new file mode 100644 index 0000000000000..3ad5bab8e070b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 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 "node.hpp" + +#include +#include +#include + +#include + +#include + +using autoware::motion_velocity_planner::MotionVelocityPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: motion_velocity_planner → test_node_ + test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); + + // set motion_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); + + test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto motion_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); + const auto motion_velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + + const auto get_motion_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_motion_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector module_names; + module_names.emplace_back("autoware::motion_velocity_planner::OutOfLaneModule"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + 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", + motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", + motion_velocity_smoother_dir + "/config/Analytical.param.yaml", + motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", + get_motion_velocity_module_config("out_of_lane")}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "motion_velocity_planner_node/input/traffic_signals"); + test_manager->publishVirtualTrafficLightState( + test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); + test_manager->publishOccupancyGrid( + test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + + // make sure motion_velocity_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 887e1167a7c3e..c40bfac40a135 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -85,7 +85,8 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/velocity_factors/surround_obstacle", "/planning/velocity_factors/traffic_light", "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway"}; + "/planning/velocity_factors/walkway", + "/planning/velocity_factors/motion_velocity_planner"}; std::vector steering_factor_topics = { "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection",