From 9d78e009b3e7be3613ce62db8e7f77f843daf829 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 9 Oct 2024 18:00:39 +0900 Subject: [PATCH 01/27] test(bpp_common): add test for object related functions (#9062) * add test for object related functions Signed-off-by: Go Sakayori * use EXPECT_DOUBLE_EQ instead of EXPECT_NEAR Signed-off-by: Go Sakayori * fix build error Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- .../test/test_utils.cpp | 2 +- .../CMakeLists.txt | 8 + .../utils/utils.hpp | 4 +- .../src/utils/utils.cpp | 17 +- .../test/test_utils.cpp | 233 ++++++++++++++++++ 5 files changed, 249 insertions(+), 15 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp index da8657c82c58e..36e2914571c1f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_utils.cpp @@ -73,7 +73,7 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, setGoal) path.points.at(4).lane_ids.push_back(5); PathWithLaneId path_with_goal; - autoware::behavior_path_planner::utils::setGoal( + autoware::behavior_path_planner::utils::set_goal( 3.5, M_PI * 0.5, path, path.points.back().point.pose, 5, &path_with_goal); // Check if skipped lane ids by smooth skip connection are filled in output path. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index 56b68304a9326..8ba0e98422212 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -33,6 +33,14 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ) if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities + test/test_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_utilities + ${PROJECT_NAME} + ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_drivable_area_expansion test/test_drivable_area_expansion.cpp test/test_footprints.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 2e13c75a46481..d6d37c3f581e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -188,7 +188,7 @@ double calcLateralDistanceFromEgoToObject( * @brief calculate longitudinal distance from ego pose to object * @return distance from ego pose to object */ -double calcLongitudinalDistanceFromEgoToObject( +double calc_longitudinal_distance_from_ego_to_object( const Pose & ego_pose, const double base_link2front, const double base_link2rear, const PredictedObject & dynamic_object); @@ -222,7 +222,7 @@ std::optional getLeftLanelet( * @param [in] goal_lane_id [unused] * @param [in] output_ptr output path with modified points for the goal */ -bool setGoal( +bool set_goal( const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index f3f6df142185b..57641306cfa2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -15,7 +15,6 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -156,7 +155,7 @@ double calcLateralDistanceFromEgoToObject( return min_distance; } -double calcLongitudinalDistanceFromEgoToObject( +double calc_longitudinal_distance_from_ego_to_object( const Pose & ego_pose, const double base_link2front, const double base_link2rear, const PredictedObject & dynamic_object) { @@ -196,18 +195,12 @@ double calcLongitudinalDistanceFromEgoToObjects( double min_distance = std::numeric_limits::max(); for (const auto & object : dynamic_objects.objects) { min_distance = std::min( - min_distance, - calcLongitudinalDistanceFromEgoToObject(ego_pose, base_link2front, base_link2rear, object)); + min_distance, calc_longitudinal_distance_from_ego_to_object( + ego_pose, base_link2front, base_link2rear, object)); } return min_distance; } -template -bool exists(std::vector vec, T element) -{ - return std::find(vec.begin(), vec.end(), element) != vec.end(); -} - std::optional findIndexOutOfGoalSearchRange( const std::vector & points, const Pose & goal, const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) @@ -253,7 +246,7 @@ std::optional findIndexOutOfGoalSearchRange( } // goal does not have z -bool setGoal( +bool set_goal( const double search_radius_range, [[maybe_unused]] const double search_rad_range, const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr) @@ -383,7 +376,7 @@ PathWithLaneId refinePathForGoal( filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; } - if (setGoal( + if (set_goal( search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, &path_with_goal)) { return path_with_goal; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp new file mode 100644 index 0000000000000..51d5fb7055a4f --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp @@ -0,0 +1,233 @@ +// 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 "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include + +#include + +using autoware::universe_utils::Point2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; + +using autoware::test_utils::createPose; +using autoware::test_utils::generateTrajectory; + +PathWithLaneId trajectory_to_path_with_lane_id(const Trajectory & trajectory) +{ + PathWithLaneId path_with_lane_id; + PathPointWithLaneId path_point_with_lane_id; + for (const auto & point : trajectory.points) { + path_point_with_lane_id.point.pose = point.pose; + path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps; + path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps; + path_with_lane_id.points.push_back(path_point_with_lane_id); + } + return path_with_lane_id; +} + +TEST(BehaviorPathPlanningUtilTest, l2Norm) +{ + using autoware::behavior_path_planner::utils::l2Norm; + + geometry_msgs::msg::Vector3 vector = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); + auto norm = l2Norm(vector); + EXPECT_DOUBLE_EQ(norm, 0.0); + + vector = autoware::universe_utils::createVector3(1.0, 2.0, 2.0); + norm = l2Norm(vector); + EXPECT_DOUBLE_EQ(norm, 3.0); +} + +TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects) +{ + using autoware::behavior_path_planner::utils::checkCollisionBetweenPathFootprintsAndObjects; + + autoware::universe_utils::LinearRing2d base_footprint = { + Point2d{1.0, 1.0}, Point2d{1.0, -1.0}, Point2d{-1.0, -1.0}, Point2d{-1.0, 1.0}, + Point2d{1.0, -1.0}}; + double margin = 0.2; + PredictedObjects objs; + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.kinematics.initial_pose_with_covariance.pose = createPose(9.0, 1.0, 0.0, 0.0, 0.0, 0.0); + objs.objects.push_back(obj); + + PathWithLaneId ego_path; + + // Condition: no path + EXPECT_FALSE( + checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); + + // Condition: object in front of path + ego_path = trajectory_to_path_with_lane_id(generateTrajectory(5, 1.0)); + EXPECT_FALSE( + checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); + + // Condition: object overlapping path + ego_path = trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0)); + EXPECT_TRUE( + checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); +} + +TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) +{ + using autoware::behavior_path_planner::utils::checkCollisionBetweenFootprintAndObjects; + + auto ego_pose = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); + autoware::universe_utils::LinearRing2d base_footprint = { + Point2d{1.0, 1.0}, Point2d{1.0, -1.0}, Point2d{-1.0, -1.0}, Point2d{-1.0, 1.0}, + Point2d{1.0, -1.0}}; + double margin = 0.2; + PredictedObjects objs; + + // Condition: no object + EXPECT_FALSE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); + + // Condition: no collision + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.kinematics.initial_pose_with_covariance.pose = createPose(9.0, 9.0, 0.0, 0.0, 0.0, 0.0); + objs.objects.push_back(obj); + EXPECT_FALSE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); + + // Condition: collision + obj.kinematics.initial_pose_with_covariance.pose.position.x = 1.0; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 1.0; + objs.objects.push_back(obj); + EXPECT_TRUE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); +} + +TEST(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) +{ + using autoware::behavior_path_planner::utils::calcLateralDistanceFromEgoToObject; + + auto ego_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + double vehicle_width = 2.0; + + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + + // Condition: overlapping + obj.kinematics.initial_pose_with_covariance.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 0.0); + + // Condition: object on left + obj.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; + EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 3.0); + + // Condition: object on right + obj.kinematics.initial_pose_with_covariance.pose.position.y = -5.0; + EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 3.0); +} + +TEST(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object) +{ + using autoware::behavior_path_planner::utils::calc_longitudinal_distance_from_ego_to_object; + + auto ego_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + PredictedObject obj; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 1.0; + + // Condition: overlapping + double base_link2front = 0.0; + double base_link2rear = 0.0; + obj.kinematics.initial_pose_with_covariance.pose = createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ( + calc_longitudinal_distance_from_ego_to_object(ego_pose, base_link2front, base_link2rear, obj), + 0.0); + + // Condition: object in front + base_link2front = 1.0; + base_link2rear = -1.0; + obj.kinematics.initial_pose_with_covariance.pose.position.x = 4.0; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_distance_from_ego_to_object(ego_pose, base_link2front, base_link2rear, obj), + 2.0); + + // Condition: object in rear + obj.kinematics.initial_pose_with_covariance.pose.position.x = -4.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_distance_from_ego_to_object(ego_pose, base_link2front, base_link2rear, obj), + 2.0); +} + +TEST(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) +{ + using autoware::behavior_path_planner::utils::calcLongitudinalDistanceFromEgoToObjects; + + auto ego_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + double base_link2front = 1.0; + double base_link2rear = -1.0; + + PredictedObjects objs; + + // Condition: none object + EXPECT_DOUBLE_EQ( + calcLongitudinalDistanceFromEgoToObjects(ego_pose, base_link2front, base_link2rear, objs), + std::numeric_limits::max()); + + // Condition: both object in front + PredictedObject near_obj; + near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 2.0, 0.0, 0.0, 0.0, 0.0); + near_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + near_obj.shape.dimensions.x = 2.0; + near_obj.shape.dimensions.y = 1.0; + + PredictedObject far_obj; + far_obj.kinematics.initial_pose_with_covariance.pose = createPose(25.0, 2.0, 0.0, 0.0, 0.0, 0.0); + far_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + far_obj.shape.dimensions.x = 5.0; + far_obj.shape.dimensions.y = 1.0; + + objs.objects.push_back(near_obj); + objs.objects.push_back(far_obj); + EXPECT_DOUBLE_EQ( + calcLongitudinalDistanceFromEgoToObjects(ego_pose, base_link2front, base_link2rear, objs), 3.0); +} + +TEST(BehaviorPathPlanningUtilTest, getHighestProbLabel) +{ + using autoware::behavior_path_planner::utils::getHighestProbLabel; + + PredictedObject obj; + ObjectClassification classification; + + // Condition: no classification + EXPECT_EQ(getHighestProbLabel(obj.classification), ObjectClassification::Type::UNKNOWN); + + // Condition: with 2 label + obj.classification.emplace_back(autoware_perception_msgs::build() + .label(ObjectClassification::CAR) + .probability(0.4)); + obj.classification.emplace_back(autoware_perception_msgs::build() + .label(ObjectClassification::TRUCK) + .probability(0.6)); + EXPECT_EQ(getHighestProbLabel(obj.classification), ObjectClassification::Type::TRUCK); +} From 3eafb9b0631436b410ac7138c33d9602b7161f43 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 9 Oct 2024 20:35:37 +0900 Subject: [PATCH 02/27] test(bpp_common): add test for parking departure utils (#9055) * add test for parking departure utils Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori * fix typo Signed-off-by: Go Sakayori * use EXPECT_DOUBLE_EQ instead of EXPECT_NEAR Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../CMakeLists.txt | 8 + .../test/test_parking_departure_utils.cpp | 315 ++++++++++++++++++ 2 files changed, 323 insertions(+) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index 8ba0e98422212..046675f357923 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -58,6 +58,14 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_parking_departure + test/test_parking_departure_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_parking_departure + ${PROJECT_NAME} + ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_turn_signal test/test_turn_signal.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp new file mode 100644 index 0000000000000..4cb1b8ef14d5a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp @@ -0,0 +1,315 @@ +// 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 "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include +#include + +#include + +constexpr double epsilon = 1e-6; + +using autoware::behavior_path_planner::PlannerData; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +using autoware::test_utils::generateTrajectory; + +PathWithLaneId trajectory_to_path_with_lane_id(const Trajectory & trajectory) +{ + PathWithLaneId path_with_lane_id; + PathPointWithLaneId path_point_with_lane_id; + for (const auto & point : trajectory.points) { + path_point_with_lane_id.point.pose = point.pose; + path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps; + path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps; + path_with_lane_id.points.push_back(path_point_with_lane_id); + } + return path_with_lane_id; +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, calcFeasibleDecelDistance) +{ + using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; + + auto data = std::make_shared(); + double velocity = 2.0; + double acceleration = 1.0; + double acceleration_limit = 2.0; + double jerk_limit = 1.0; + double target_velocity = 3.0; + + auto odometry = std::make_shared(); + odometry->pose.pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + odometry->twist.twist.linear.x = velocity; + auto accel_with_covariance = + std::make_shared(); + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + auto planner_data = std::static_pointer_cast(data); + + // condition: current velocity is slower than target velocity + auto distance = + calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity); + ASSERT_TRUE(distance.has_value()); + EXPECT_DOUBLE_EQ(distance.value(), 0.0); + + // condition: calculates deceleration distance + velocity = 5.0; + odometry->twist.twist.linear.x = velocity; + data->self_odometry = odometry; + planner_data = std::static_pointer_cast(data); + distance = + calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity); + ASSERT_TRUE(distance.has_value()); + EXPECT_NEAR(distance.value(), 18.7730133, epsilon); + + // condition: not valid condition + velocity = 0.3; + target_velocity = 0.0; + acceleration = -1.5; + jerk_limit = 0.25; + acceleration_limit = 2.0; + odometry->twist.twist.linear.x = velocity; + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + planner_data = std::static_pointer_cast(data); + distance = + calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity); + ASSERT_FALSE(distance.has_value()); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, modifyVelocityByDirection) +{ + using autoware::behavior_path_planner::utils::parking_departure::modifyVelocityByDirection; + + const double velocity = 3.0; + const double target_velocity = 2.0; + const double acceleration = 2.0; + + std::vector paths; + auto short_path = trajectory_to_path_with_lane_id(generateTrajectory(1, 1.0)); + auto long_path = + trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0, -velocity)); + auto reverse_path = + trajectory_to_path_with_lane_id(generateTrajectory(10, -1.0, velocity, -M_PI)); + + paths.push_back(short_path); + paths.push_back(long_path); + paths.push_back(reverse_path); + + std::vector> terminal_vel_acc_pairs; + terminal_vel_acc_pairs.emplace_back(0.5, 0.5); + terminal_vel_acc_pairs.emplace_back(1.0, 1.0); + terminal_vel_acc_pairs.emplace_back(1.5, 1.5); + + modifyVelocityByDirection(paths, terminal_vel_acc_pairs, target_velocity, acceleration); + + // condition: number of point less than 2 + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(0).first, 0.0); + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(0).second, 0.0); + + // condition: forward driving + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(1).first, target_velocity); + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(1).second, acceleration); + for (const auto & point : paths.at(1).points) { + if (point == paths.at(1).points.back()) + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + else + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity); + } + + // condition: reverse driving + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(2).first, -target_velocity); + EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(2).second, -acceleration); + for (const auto & point : paths.at(2).points) { + if (point == paths.at(2).points.back()) + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + else + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, -velocity); + } +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, updatePathProperty) +{ + using autoware::behavior_path_planner::utils::parking_departure::EgoPredictedPathParams; + using autoware::behavior_path_planner::utils::parking_departure::updatePathProperty; + + auto params = std::make_shared(); + params->min_acceleration = 1.0; + params->acceleration = 1.5; + params->max_velocity = 0.0; + auto pair_terminal_velocity_and_accel = std::make_pair(3.0, 2.0); + + updatePathProperty(params, pair_terminal_velocity_and_accel); + EXPECT_DOUBLE_EQ(params->max_velocity, 3.0); + EXPECT_DOUBLE_EQ(params->acceleration, 2.0); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, initializeCollisionCheckDebugMap) +{ + using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; + + autoware::behavior_path_planner::CollisionCheckDebugMap debug_map; + auto uuid1 = autoware::universe_utils::toBoostUUID(autoware::universe_utils::generateUUID()); + autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug debug_info; + debug_map[uuid1] = debug_info; + + initializeCollisionCheckDebugMap(debug_map); + ASSERT_TRUE(debug_map.empty()); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, getPairsTerminalVelocityAndAccel) +{ + using autoware::behavior_path_planner::utils::parking_departure::getPairsTerminalVelocityAndAccel; + + std::vector> pairs_terminal_velocity_and_accel; + pairs_terminal_velocity_and_accel.emplace_back(2.0, 1.0); + pairs_terminal_velocity_and_accel.emplace_back(0.05, -1.0); + + // condition: current path idx exceeds pairs size + auto pair = getPairsTerminalVelocityAndAccel(pairs_terminal_velocity_and_accel, 2); + EXPECT_DOUBLE_EQ(pair.first, 0.0); + EXPECT_DOUBLE_EQ(pair.first, 0.0); + + // condition: get current idx pair + pair = getPairsTerminalVelocityAndAccel(pairs_terminal_velocity_and_accel, 1); + EXPECT_DOUBLE_EQ(pair.first, 0.05); + EXPECT_DOUBLE_EQ(pair.second, -1.0); +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, generateFeasibleStopPath) +{ + using autoware::behavior_path_planner::utils::parking_departure::generateFeasibleStopPath; + + auto data = std::make_shared(); + double velocity = 0.3; + double acceleration = -1.5; + double maximum_jerk = 0.25; + double maximum_deceleration = 2.0; + + auto odometry = std::make_shared(); + odometry->pose.pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + odometry->twist.twist.linear.x = velocity; + auto accel_with_covariance = + std::make_shared(); + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + auto planner_data = std::static_pointer_cast(data); + + std::optional stop_pose; + + // condition: empty path + PathWithLaneId path; + auto stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + EXPECT_FALSE(stop_path.has_value()); + + // condition: not valid condition for stop distance + path = trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0, velocity)); + stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + EXPECT_FALSE(stop_path.has_value()); + + // condition: not valid condition for stop index + velocity = 5.0; + acceleration = 1.0; + odometry->twist.twist.linear.x = velocity; + accel_with_covariance->accel.accel.linear.x = acceleration; + data->self_odometry = odometry; + data->self_acceleration = accel_with_covariance; + planner_data = std::static_pointer_cast(data); + path = trajectory_to_path_with_lane_id(generateTrajectory(10, 1.0, velocity)); + stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + EXPECT_FALSE(stop_path.has_value()); + + // condition: valid condition + maximum_jerk = 5.0; + maximum_deceleration = -3.0; + stop_path = + generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk); + size_t i = 0; + ASSERT_TRUE(stop_path.has_value()); + for (const auto & point : stop_path->points) { + if (i < 7) + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity); + else + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + i++; + } +} + +TEST(BehaviorPathPlanningParkingDepartureUtil, calcEndArcLength) +{ + using autoware::behavior_path_planner::utils::parking_departure::calcEndArcLength; + + lanelet::LineString3d left_bound; + lanelet::LineString3d right_bound; + + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1}); + left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1}); + right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1}); + lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound}; + + lanelet::LineString3d centerline; + centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0}); + centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0}); + lanelet.setCenterline(centerline); + + lanelet::ConstLanelets road_lanes = {lanelet}; + + double s_start = 0.2; + double forward_path_length = 0.1; + auto goal_pose = autoware::test_utils::createPose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0); + + // condition: goal pose not in lanelets + auto end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length); + EXPECT_FALSE(end_arc.second); + + // condition: goal pose behind start + goal_pose.position.x = -0.9; + goal_pose.position.y = 0.0; + end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length); + EXPECT_FALSE(end_arc.second); + + // condition: goal pose beyond start + goal_pose.position.x = 0.0; + end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length); + EXPECT_FALSE(end_arc.second); + + // condition: path end is goal + goal_pose.position.x = -0.75; + end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + EXPECT_DOUBLE_EQ(end_arc.first, 0.25); + EXPECT_TRUE(end_arc.second); +} From 41e08dceadb64374f49cc0411ada9c63dab96765 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 10 Oct 2024 03:46:47 +0900 Subject: [PATCH 03/27] refactor(goal_planner): remove modified_goal in ThreadDafeData (#9010) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 7 +- .../pull_over_planner_base.hpp | 4 +- .../thread_data.hpp | 13 -- .../src/goal_planner_module.cpp | 135 ++++++++++-------- 4 files changed, 80 insertions(+), 79 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 70c9fd528b6a5..93fc38f261117 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -372,10 +372,13 @@ class GoalPlannerModule : public SceneModuleInterface bool isStopped( std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); - bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const; + bool isOnModifiedGoal( + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters) const; double calcModuleRequestLength() const; bool needPathUpdate( const Pose & current_pose, const double path_update_duration, + const std::optional & modified_goal_opt, const GoalPlannerParameters & parameters) const; bool isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, @@ -406,7 +409,7 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data); - std::optional> selectPullOverPath( + std::optional selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 059d76530915d..2a328a467723c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -54,8 +54,8 @@ struct PullOverPath size_t goal_id() const { return modified_goal_pose_.id; } size_t id() const { return id_; } Pose start_pose() const { return start_pose_; } - Pose end_pose() const { return modified_goal_pose_.goal_pose; } - GoalCandidate modified_goal_pose() const { return modified_goal_pose_; } + Pose modified_goal_pose() const { return modified_goal_pose_.goal_pose; } + GoalCandidate modified_goal() const { return modified_goal_pose_; } std::vector & partial_paths() { return partial_paths_; } const std::vector & partial_paths() const { return partial_paths_; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index 3bfaa90b845fb..2d3dd30bdf7ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -105,22 +105,12 @@ class ThreadSafeData return true; } - std::optional getPullOverPlannerType() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return std::nullopt; - } - return pull_over_path_->type(); - }; - void reset() { const std::lock_guard lock(mutex_); pull_over_path_ = nullptr; pull_over_path_candidates_.clear(); goal_candidates_.clear(); - modified_goal_pose_ = std::nullopt; last_path_update_time_ = std::nullopt; last_path_idx_increment_time_ = std::nullopt; closest_start_pose_ = std::nullopt; @@ -135,7 +125,6 @@ class ThreadSafeData DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) DEFINE_SETTER_GETTER_WITH_MUTEX( @@ -168,7 +157,6 @@ class ThreadSafeData void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg) { @@ -179,7 +167,6 @@ class ThreadSafeData std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; GoalCandidates goal_candidates_{}; - std::optional modified_goal_pose_; std::optional last_path_update_time_; std::optional last_path_idx_increment_time_; std::optional closest_start_pose_{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 065e27b47227e..03d5dfdcf2a24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -259,7 +259,17 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const auto current_state = thread_safe_data_.get_prev_data().state; const bool need_update = std::invoke([&]() { - if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path_opt = + found_pull_over_path + ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + const std::optional modified_goal_opt = + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) + : std::nullopt; + if (isOnModifiedGoal( + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } if (hasDeviatedFromCurrentPreviousModulePath(local_planner_data, previous_module_output)) { @@ -408,7 +418,16 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } - if (isOnModifiedGoal(local_planner_data->self_odometry->pose.pose, parameters)) { + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path_opt = + found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + const std::optional modified_goal_opt = + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) + : std::nullopt; + if (isOnModifiedGoal( + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return; } @@ -421,7 +440,8 @@ void GoalPlannerModule::onFreespaceParkingTimer() local_planner_data, occupancy_grid_map, parameters) && is_new_costmap && needPathUpdate( - local_planner_data->self_odometry->pose.pose, path_update_duration, parameters)) { + local_planner_data->self_odometry->pose.pose, path_update_duration, modified_goal_opt, + parameters)) { planFreespacePath(local_planner_data, goal_searcher, occupancy_grid_map); } } @@ -543,14 +563,21 @@ void GoalPlannerModule::updateData() found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) : std::nullopt; + const auto modified_goal_pose = [&]() -> std::optional { + if (!pull_over_path_recv) { + return std::nullopt; + } + const auto & pull_over_path = pull_over_path_recv.value(); + return pull_over_path.modified_goal(); + }(); + const bool is_current_safe = isSafePath( planner_data_, found_pull_over_path, pull_over_path_recv, *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, - thread_safe_data_.get_modified_goal_pose(), planner_data_, occupancy_grid_map_, is_current_safe, - *parameters_, goal_searcher_, isActivated(), pull_over_path_recv, - debug_data_.ego_polygons_expanded); + modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, + goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, @@ -811,7 +838,6 @@ bool GoalPlannerModule::planFreespacePath( { const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path(*freespace_path); - thread_safe_data_.set_modified_goal_pose(goal_candidate); debug_data_.freespace_planner.is_planning = false; } @@ -905,7 +931,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() return fixed_goal_planner_->plan(planner_data_); } -std::optional> GoalPlannerModule::selectPullOverPath( +std::optional GoalPlannerModule::selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const @@ -1130,7 +1156,7 @@ std::optional> GoalPlannerModule::selectP checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id()))); + return path; } return {}; } @@ -1227,7 +1253,7 @@ void GoalPlannerModule::setModifiedGoal( if (context_data.pull_over_path_opt) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose; + modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose(); modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -1349,12 +1375,17 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( const bool is_freespace = pull_over_path_with_velocity_opt && pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; + const std::optional modified_goal_opt = + pull_over_path_with_velocity_opt + ? std::make_optional(pull_over_path_with_velocity_opt.value().modified_goal()) + : std::nullopt; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && !is_freespace && needPathUpdate( - planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { + planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, modified_goal_opt, + *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ @@ -1369,12 +1400,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // Select a path that is as safe as possible and has a high priority. const auto & pull_over_path_candidates = context_data.pull_over_path_candidates; - const auto path_and_goal_opt = + auto path_and_goal_opt = selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); // update thread_safe_data_ if (path_and_goal_opt) { - auto [pull_over_path, modified_goal] = *path_and_goal_opt; + const auto & pull_over_path = path_and_goal_opt.value(); /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of @@ -1385,7 +1416,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( * As the next action item, only set this selected pull_over_path to only * FreespaceThreadSafeData. */ - thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); + thread_safe_data_.set(goal_candidates, pull_over_path); if (pull_over_path_with_velocity_opt) { auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); // copy the path for later setOutput() @@ -1395,7 +1426,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( } RCLCPP_DEBUG( getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), - modified_goal.id); + pull_over_path.modified_goal().id); } else { thread_safe_data_.set(goal_candidates); } @@ -1413,11 +1444,6 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // For debug setDebugData(context_data_with_velocity); - if (parameters_->print_debug_info) { - // For evaluations - printParkingPositionError(); - } - if (!pull_over_path_with_velocity_opt) { return output; } @@ -1456,8 +1482,7 @@ void GoalPlannerModule::postProcess() } updateSteeringFactor( - context_data, - {pull_over_path.start_pose(), thread_safe_data_.get_modified_goal_pose()->goal_pose}, + context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()}, {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); @@ -1509,10 +1534,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, pull_over_path.start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); + full_path.points, pull_over_path.modified_goal_pose().position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - thread_safe_data_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); + pull_over_path.modified_goal_pose().position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1695,8 +1720,16 @@ bool GoalPlannerModule::isStuck( const std::shared_ptr occupancy_grid_map, const GoalPlannerParameters & parameters) { + const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const std::optional pull_over_path_opt = + found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) + : std::nullopt; + const std::optional modified_goal_opt = + pull_over_path_opt + ? std::make_optional(pull_over_path_opt.value().modified_goal()) + : std::nullopt; const std::lock_guard lock(mutex_); - if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, parameters)) { + if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } @@ -1705,19 +1738,13 @@ bool GoalPlannerModule::isStuck( return false; } - // not found safe path - if (!thread_safe_data_.foundPullOverPath()) { + if (!found_pull_over_path) { return true; } - // any path has never been found - if (!thread_safe_data_.get_pull_over_path()) { - return false; - } - - const auto pull_over_path = thread_safe_data_.get_pull_over_path(); + const auto & pull_over_path = pull_over_path_opt.value(); if (parameters.use_object_recognition) { - const auto path = pull_over_path->getCurrentPath(); + const auto & path = pull_over_path.getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); if (goal_planner_utils::checkObjectsCollision( path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters, @@ -1784,13 +1811,14 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d } bool GoalPlannerModule::isOnModifiedGoal( - const Pose & current_pose, const GoalPlannerParameters & parameters) const + const Pose & current_pose, const std::optional & modified_goal_opt, + const GoalPlannerParameters & parameters) const { - if (!thread_safe_data_.get_modified_goal_pose()) { + if (!modified_goal_opt) { return false; } - return calcDistance2d(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose) < + return calcDistance2d(current_pose, modified_goal_opt.value().goal_pose) < parameters.th_arrived_distance; } @@ -1808,7 +1836,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & start_pose = pull_over_path.start_pose(); - const auto & end_pose = pull_over_path.end_pose(); + const auto & end_pose = pull_over_path.modified_goal_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -2126,7 +2154,7 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) { const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); const Pose & start_pose = pull_over_path.start_pose(); - const Pose & end_pose = pull_over_path.end_pose(); + const Pose & end_pose = pull_over_path.modified_goal_pose(); return isCrossingPossible(start_pose, end_pose, lanes); } @@ -2391,7 +2419,8 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) const auto & pull_over_path = context_data.pull_over_path_opt.value(); add( createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); - add(createPoseMarkerArray(pull_over_path.end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPoseMarkerArray( + pull_over_path.modified_goal_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); add(createPathMarkerArray(pull_over_path.full_path(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); @@ -2497,8 +2526,8 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = thread_safe_data_.get_modified_goal_pose() - ? thread_safe_data_.get_modified_goal_pose()->goal_pose + marker.pose = context_data.pull_over_path_opt + ? context_data.pull_over_path_opt.value().modified_goal_pose() : planner_data_->self_odometry->pose.pose; if (context_data.pull_over_path_opt) { const auto & pull_over_path = context_data.pull_over_path_opt.value(); @@ -2544,30 +2573,12 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } -void GoalPlannerModule::printParkingPositionError() const -{ - const auto current_pose = planner_data_->self_odometry->pose.pose; - const double real_shoulder_to_map_shoulder = 0.0; - - const Pose goal_to_ego = - inverseTransformPose(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose); - const double dx = goal_to_ego.position.x; - const double dy = goal_to_ego.position.y; - const double distance_from_real_shoulder = - real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy; - RCLCPP_INFO( - getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, - autoware::universe_utils::rad2deg( - tf2::getYaw(current_pose.orientation) - - tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), - distance_from_real_shoulder); -} - bool GoalPlannerModule::needPathUpdate( const Pose & current_pose, const double path_update_duration, + const std::optional & modified_goal_opt, const GoalPlannerParameters & parameters) const { - return !isOnModifiedGoal(current_pose, parameters) && + return !isOnModifiedGoal(current_pose, modified_goal_opt, parameters) && hasEnoughTimePassedSincePathUpdate(path_update_duration); } From f49e7a6c1b7a978a5b82a7dfece9dbeb063a15d2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 10 Oct 2024 09:38:53 +0900 Subject: [PATCH 04/27] chore(autoware_ground_segmentation): scan ground filter refactoring (#9061) * chore: Add comment classification logic for point cloud grid scan Signed-off-by: Taekjin LEE * chore: renamed horizontal angle to azimuth angle Signed-off-by: Taekjin LEE * chore: rename offset to data_index Signed-off-by: Taekjin LEE * chore: rename ground_cluster to centroid_bin chore: Refactor recheckGroundCluster function in scan_ground_filter Signed-off-by: Taekjin LEE * chore: rename too short variables Signed-off-by: Taekjin LEE * refactor: set input to be const Signed-off-by: Taekjin LEE * refactor: update functions to be const Signed-off-by: Taekjin LEE * chore: reorder params Signed-off-by: Taekjin LEE * refactor: Add ScanGroundGrid class for managing grid data Signed-off-by: Taekjin LEE * refactor: Update grid parameters and calculations in ScanGroundGrid class Signed-off-by: Taekjin LEE * refactor: remove unused methods Signed-off-by: Taekjin LEE * refactor: classification description Signed-off-by: Taekjin LEE * refactor: initialize members in ScanGroundGrid class Signed-off-by: Taekjin LEE * refactor: remove unused value Signed-off-by: Taekjin LEE * chore: reduce scope Signed-off-by: Taekjin LEE * refactor: align structure between convertPointcloud and convertPointcloudGridScan Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../docs/scan-ground-filter.md | 18 +- .../src/scan_ground_filter/grid.hpp | 111 ++++ .../src/scan_ground_filter/node.cpp | 505 +++++++++--------- .../src/scan_ground_filter/node.hpp | 105 ++-- 4 files changed, 432 insertions(+), 307 deletions(-) create mode 100644 perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp diff --git a/perception/autoware_ground_segmentation/docs/scan-ground-filter.md b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md index 392bed89ac902..6b2f6d6b57571 100644 --- a/perception/autoware_ground_segmentation/docs/scan-ground-filter.md +++ b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md @@ -8,14 +8,16 @@ The purpose of this node is that remove the ground points from the input pointcl This algorithm works by following steps, -1. Divide whole pointclouds into groups by horizontal angle and sort by xy-distance. -2. Divide sorted pointclouds of each ray into grids -3. Check the xy distance to previous pointcloud, if the distance is large and previous pointcloud is "no ground" and the height level of current point greater than previous point, the current pointcloud is classified as no ground. -4. Check vertical angle of the point compared with previous ground grid -5. Check the height of the point compared with predicted ground level -6. If vertical angle is greater than local_slope_max and related height to predicted ground level is greater than "non ground height threshold", the point is classified as "non ground" -7. If the vertical angle is in range of [-local_slope_max, local_slope_max] or related height to predicted ground level is smaller than non_ground_height_threshold, the point is classified as "ground" -8. If the vertical angle is lower than -local_slope_max or the related height to ground level is greater than detection_range_z_max, the point will be classified as out of range +1. Divide whole pointclouds into groups by azimuth angle (so-called ray) +2. Sort points by radial distance (xy-distance), on each ray. +3. Divide pointcloud into grids, on each ray. +4. Classify the point + 1. Check radial distance to previous pointcloud, if the distance is large and previous pointcloud is "no ground" and the height level of current point greater than previous point, the current pointcloud is classified as no ground. + 2. Check vertical angle of the point compared with previous ground grid + 3. Check the height of the point compared with predicted ground level + 4. If vertical angle is greater than local_slope_max and related height to predicted ground level is greater than "non ground height threshold", the point is classified as "non ground" + 5. If the vertical angle is in range of [-local_slope_max, local_slope_max] or related height to predicted ground level is smaller than non_ground_height_threshold, the point is classified as "ground" + 6. If the vertical angle is lower than -local_slope_max or the related height to ground level is greater than detection_range_z_max, the point will be classified as out of range ## Inputs / Outputs diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp new file mode 100644 index 0000000000000..cd55d4a548211 --- /dev/null +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/grid.hpp @@ -0,0 +1,111 @@ +// 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 SCAN_GROUND_FILTER__GRID_HPP_ +#define SCAN_GROUND_FILTER__GRID_HPP_ + +#include +#include + +#include + +namespace autoware::ground_segmentation +{ + +class ScanGroundGrid +{ +public: + ScanGroundGrid() = default; + ~ScanGroundGrid() = default; + + void initialize( + const float grid_size_m, const float grid_mode_switch_radius, const float virtual_lidar_z) + { + grid_size_m_ = grid_size_m; + mode_switch_radius_ = grid_mode_switch_radius; + virtual_lidar_z_ = virtual_lidar_z; + + // calculate parameters + inv_grid_size_m_ = 1.0f / grid_size_m_; + mode_switch_grid_id_ = mode_switch_radius_ * inv_grid_size_m_; + mode_switch_angle_rad_ = std::atan2(mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = universe_utils::normalizeRadian( + std::atan2(mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + universe_utils::normalizeRadian(mode_switch_angle_rad_); + inv_grid_size_rad_ = 1.0f / grid_size_rad_; + tan_grid_size_rad_ = std::tan(grid_size_rad_); + grid_id_offset_ = mode_switch_grid_id_ - mode_switch_angle_rad_ * inv_grid_size_rad_; + + is_initialized_ = true; + } + + float getGridSize(const float radius, const size_t grid_id) const + { + // check if initialized + if (!is_initialized_) { + throw std::runtime_error("ScanGroundGrid is not initialized."); + } + + float grid_size = grid_size_m_; + constexpr uint16_t back_steps_num = 1; + + if (radius > mode_switch_radius_ && grid_id > mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ + // where gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = radius - (radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; + } + + uint16_t getGridId(const float radius) const + { + // check if initialized + if (!is_initialized_) { + throw std::runtime_error("ScanGroundGrid is not initialized."); + } + + uint16_t grid_id = 0; + if (radius <= mode_switch_radius_) { + grid_id = static_cast(radius * inv_grid_size_m_); + } else { + auto gamma{universe_utils::normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset_ + gamma * inv_grid_size_rad_; + } + return grid_id; + } + +private: + bool is_initialized_ = false; + + // configured parameters + float grid_size_m_ = 0.0f; + float mode_switch_radius_ = 0.0f; + float virtual_lidar_z_ = 0.0f; + + // calculated parameters + float inv_grid_size_m_ = 0.0f; + float grid_size_rad_ = 0.0f; + float inv_grid_size_rad_ = 0.0f; + float tan_grid_size_rad_ = 0.0f; + float mode_switch_grid_id_ = 0.0f; + float mode_switch_angle_rad_ = 0.0f; + float grid_id_offset_ = 0.0f; +}; + +} // namespace autoware::ground_segmentation + +#endif // SCAN_GROUND_FILTER__GRID_HPP_ diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 72481d13c41fb..c0419c7121c64 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -14,10 +14,10 @@ #include "node.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include +#include +#include +#include #include #include @@ -38,40 +38,48 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = declare_parameter("low_priority_region_x"); - detection_range_z_max_ = declare_parameter("detection_range_z_max"); - center_pcl_shift_ = declare_parameter("center_pcl_shift"); - non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); - grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); - - grid_size_m_ = declare_parameter("grid_size_m"); - gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + // modes elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + + // common parameters + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); + + // common thresholds global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); split_points_distance_tolerance_square_ = split_points_distance_tolerance_ * split_points_distance_tolerance_; - split_height_distance_ = declare_parameter("split_height_distance"); + + // vehicle info + vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); + + // non-grid parameters use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + split_height_distance_ = declare_parameter("split_height_distance"); + + // grid mode parameters use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); use_lowest_point_ = declare_parameter("use_lowest_point"); - radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); - vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); - grid_mode_switch_grid_id_ = - grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division + // grid parameters + grid_size_m_ = declare_parameter("grid_size_m"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); virtual_lidar_z_ = vehicle_info_.vehicle_height_m; - grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - tan_grid_size_rad_ = std::tan(grid_size_rad_); - offset_initialized_ = false; + // initialize grid + grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); + + // data access + data_offset_initialized_ = false; } using std::placeholders::_1; @@ -98,31 +106,32 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } -inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstPtr & input) +inline void ScanGroundFilterComponent::set_field_index_offsets(const PointCloud2ConstPtr & input) { - x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; - y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; - z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + data_offset_x_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + data_offset_y_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + data_offset_z_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; int intensity_index = pcl::getFieldIndex(*input, "intensity"); if (intensity_index != -1) { - intensity_offset_ = input->fields[intensity_index].offset; + data_offset_intensity_ = input->fields[intensity_index].offset; intensity_type_ = input->fields[intensity_index].datatype; } else { - intensity_offset_ = -1; + data_offset_intensity_ = -1; } - offset_initialized_ = true; + data_offset_initialized_ = true; } -inline void ScanGroundFilterComponent::get_point_from_global_offset( - const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +inline void ScanGroundFilterComponent::get_point_from_data_index( + const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const { - point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); - point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); - point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); + point.x = *reinterpret_cast(&input->data[data_index + data_offset_x_]); + point.y = *reinterpret_cast(&input->data[data_index + data_offset_y_]); + point.z = *reinterpret_cast(&input->data[data_index + data_offset_z_]); } void ScanGroundFilterComponent::convertPointcloudGridScan( - const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -131,52 +140,41 @@ void ScanGroundFilterComponent::convertPointcloudGridScan( PointData current_point; const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; - const auto inv_grid_size_rad = 1.0f / grid_size_rad_; - const auto inv_grid_size_m = 1.0f / grid_size_m_; - - const auto grid_id_offset = - grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; const size_t in_cloud_data_size = in_cloud->data.size(); const size_t in_cloud_point_step = in_cloud->point_step; - { // grouping pointcloud by its horizontal angle + { // grouping pointcloud by its azimuth angle std::unique_ptr inner_st_ptr; if (time_keeper_) - inner_st_ptr = std::make_unique("horizontal_angle_grouping", *time_keeper_); + inner_st_ptr = std::make_unique("azimuth_angle_grouping", *time_keeper_); size_t point_index = 0; pcl::PointXYZ input_point; - for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; - global_offset += in_cloud_point_step) { - get_point_from_global_offset(in_cloud, input_point, global_offset); + for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; + data_index += in_cloud_point_step) { + // Get Point + get_point_from_data_index(in_cloud, data_index, input_point); + // determine the azimuth angle group auto x{input_point.x - x_shift}; // base on front wheel center auto radius{static_cast(std::hypot(x, input_point.y))}; auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; - - // divide by vertical angle auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; - uint16_t grid_id = 0; - if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius * inv_grid_size_m); - } else { - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - grid_id = grid_id_offset + gamma * inv_grid_size_rad; - } - current_point.grid_id = grid_id; + current_point.radius = radius; current_point.point_state = PointLabel::INIT; current_point.orig_index = point_index; + current_point.grid_id = grid_.getGridId(radius); - // radial divisions + // store the point in the corresponding radial division out_radial_ordered_points[radial_div].emplace_back(current_point); ++point_index; } } - { // sorting pointcloud by distance, on each horizontal angle group + { // sorting pointcloud by distance, on each azimuth angle group std::unique_ptr inner_st_ptr; if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); @@ -189,7 +187,8 @@ void ScanGroundFilterComponent::convertPointcloudGridScan( } void ScanGroundFilterComponent::convertPointcloud( - const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -202,18 +201,19 @@ void ScanGroundFilterComponent::convertPointcloud( const size_t in_cloud_data_size = in_cloud->data.size(); const size_t in_cloud_point_step = in_cloud->point_step; - { // grouping pointcloud by its horizontal angle + { // grouping pointcloud by its azimuth angle std::unique_ptr inner_st_ptr; if (time_keeper_) - inner_st_ptr = std::make_unique("horizontal_angle_grouping", *time_keeper_); + inner_st_ptr = std::make_unique("azimuth_angle_grouping", *time_keeper_); size_t point_index = 0; pcl::PointXYZ input_point; - for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; - global_offset += in_cloud_point_step) { - // Point - get_point_from_global_offset(in_cloud, input_point, global_offset); + for (size_t data_index = 0; data_index + in_cloud_point_step <= in_cloud_data_size; + data_index += in_cloud_point_step) { + // Get Point + get_point_from_data_index(in_cloud, data_index, input_point); + // determine the azimuth angle group auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; @@ -222,13 +222,13 @@ void ScanGroundFilterComponent::convertPointcloud( current_point.point_state = PointLabel::INIT; current_point.orig_index = point_index; - // radial divisions + // store the point in the corresponding radial division out_radial_ordered_points[radial_div].emplace_back(current_point); ++point_index; } } - { // sorting pointcloud by distance, on each horizontal angle group + { // sorting pointcloud by distance, on each azimuth angle group std::unique_ptr inner_st_ptr; if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); @@ -240,30 +240,15 @@ void ScanGroundFilterComponent::convertPointcloud( } } -void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) +void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) const { point.x = vehicle_info_.wheel_base_m; point.y = 0; point.z = 0; } -inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) -{ - float grid_size = grid_size_m_; - uint16_t back_steps_num = 1; - - if ( - p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { - // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * - // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) - grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / - (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); - } - return grid_size; -} - void ScanGroundFilterComponent::initializeFirstGndGrids( - const float h, const float r, const uint16_t id, std::vector & gnd_grids) + const float h, const float r, const uint16_t id, std::vector & gnd_grids) const { GridCenter curr_gnd_grid; for (int ind_grid = id - 1 - gnd_grid_buffer_size_; ind_grid < id - 1; ++ind_grid) { @@ -282,7 +267,8 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const { float next_gnd_z = 0.0f; float curr_gnd_slope_ratio = 0.0f; @@ -307,56 +293,58 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( curr_gnd_slope_ratio = curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (pd.radius - gnd_buff_radius) + gnd_buff_z_mean; - float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); + float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (pd.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; - tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; + tmp_delta_mean_z = point_curr.z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_radius = pd.radius - (gnd_grids_list.end() - 2)->radius; float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(point_curr.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || abs(local_slope_ratio) <= local_slope_max_ratio_) { - p.point_state = PointLabel::GROUND; - } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { - p.point_state = PointLabel::NON_GROUND; + pd.point_state = PointLabel::GROUND; + } else if (point_curr.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + pd.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const { - float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; - float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; + float tmp_delta_max_z = point_curr.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = point_curr.z - gnd_grids_list.back().avg_height; + float tmp_delta_radius = pd.radius - gnd_grids_list.back().radius; float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { - p.point_state = PointLabel::GROUND; + pd.point_state = PointLabel::GROUND; } else if (local_slope_ratio > global_slope_max_ratio_) { - p.point_state = PointLabel::NON_GROUND; + pd.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const { - float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; - float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; + float tmp_delta_avg_z = point_curr.z - gnd_grids_list.back().avg_height; + float tmp_delta_radius = pd.radius - gnd_grids_list.back().radius; float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if (abs(local_slope_ratio) < global_slope_max_ratio_) { - p.point_state = PointLabel::GROUND; + pd.point_state = PointLabel::GROUND; } else if (local_slope_ratio > global_slope_max_ratio_) { - p.point_state = PointLabel::NON_GROUND; + pd.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, - pcl::PointIndices & non_ground_indices) + const PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, + pcl::PointIndices & non_ground_indices) const { float reference_height = use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight(); @@ -370,131 +358,157 @@ void ScanGroundFilterComponent::recheckGroundCluster( } void ScanGroundFilterComponent::classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices) + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); out_no_ground_indices.indices.clear(); + + // run the classification algorithm for each ray (azimuth division) for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { - PointsCentroid ground_cluster; - ground_cluster.initialize(); + PointsCentroid centroid_bin; + centroid_bin.initialize(); std::vector gnd_grids; - GridCenter curr_gnd_grid; // check empty ray if (in_radial_ordered_clouds[i].size() == 0) { continue; } - // check the first point in ray - auto * p = &in_radial_ordered_clouds[i][0]; - bool initialized_first_gnd_grid = false; bool prev_list_init = false; - pcl::PointXYZ p_orig_point, prev_p_orig_point; - for (auto & point : in_radial_ordered_clouds[i]) { - auto * prev_p = p; // for checking the distance to prev point - prev_p_orig_point = p_orig_point; - p = &point; - get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); - float global_slope_ratio_p = p_orig_point.z / p->radius; + + PointData pd_curr, pd_prev; + pcl::PointXYZ point_curr, point_prev; + + // initialize the previous point + pd_curr = in_radial_ordered_clouds[i][0]; + + // iterate over the points in the ray + for (const auto & point : in_radial_ordered_clouds[i]) { + // set the previous point + point_prev = point_curr; + pd_prev = pd_curr; + + // set the current point + pd_curr = point; + const size_t data_index = in_cloud->point_step * pd_curr.orig_index; + get_point_from_data_index(in_cloud, data_index, point_curr); + + // set the thresholds + const float global_slope_ratio_p = point_curr.z / pd_curr.radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p_orig_point.x < low_priority_region_x_) { + if (point_curr.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); - } - // classify first grid's point cloud - if ( - !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && - p_orig_point.z > non_ground_height_threshold_local) { - out_no_ground_indices.indices.push_back(p->orig_index); - p->point_state = PointLabel::NON_GROUND; - continue; - } - - if ( - !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && - abs(p_orig_point.z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); - p->point_state = PointLabel::GROUND; - initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - continue; + non_ground_height_threshold_ * abs(point_curr.x / low_priority_region_x_); } if (!initialized_first_gnd_grid) { + // classify first grid's point cloud + if ( + global_slope_ratio_p >= global_slope_max_ratio_ && + point_curr.z > non_ground_height_threshold_local) { + out_no_ground_indices.indices.push_back(pd_curr.orig_index); + pd_curr.point_state = PointLabel::NON_GROUND; + } else if ( + abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(point_curr.z) < non_ground_height_threshold_local) { + centroid_bin.addPoint(pd_curr.radius, point_curr.z, pd_curr.orig_index); + pd_curr.point_state = PointLabel::GROUND; + // if the gird id is not the initial grid_id, then the first gnd grid is initialized + initialized_first_gnd_grid = static_cast(pd_curr.grid_id - pd_prev.grid_id); + } + // else, the point is not classified continue; } - // initialize lists of previous gnd grids + // initialize gnd_grids based on the initial centroid_bin if (!prev_list_init) { - float h = ground_cluster.getAverageHeight(); - float r = ground_cluster.getAverageRadius(); - initializeFirstGndGrids(h, r, p->grid_id, gnd_grids); + float h = centroid_bin.getAverageHeight(); + float r = centroid_bin.getAverageRadius(); + initializeFirstGndGrids(h, r, pd_curr.grid_id, gnd_grids); prev_list_init = true; } - // move to new grid - if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { + // finalize the current centroid_bin and update the gnd_grids + if (pd_curr.grid_id > pd_prev.grid_id && centroid_bin.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud if (use_recheck_ground_cluster_) { recheckGroundCluster( - ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); + centroid_bin, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); + // centroid_bin is not modified. should be rechecked by out_no_ground_indices? } - curr_gnd_grid.radius = ground_cluster.getAverageRadius(); - curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); - curr_gnd_grid.max_height = ground_cluster.getMaxHeight(); - curr_gnd_grid.grid_id = prev_p->grid_id; + // convert the centroid_bin to grid-center and add it to the gnd_grids + GridCenter curr_gnd_grid; + curr_gnd_grid.radius = centroid_bin.getAverageRadius(); + curr_gnd_grid.avg_height = centroid_bin.getAverageHeight(); + curr_gnd_grid.max_height = centroid_bin.getMaxHeight(); + curr_gnd_grid.grid_id = pd_prev.grid_id; gnd_grids.push_back(curr_gnd_grid); - ground_cluster.initialize(); + // clear the centroid_bin + centroid_bin.initialize(); } - // classify - if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { - p->point_state = PointLabel::OUT_OF_RANGE; + + // 1: height is out-of-range + if (point_curr.z - gnd_grids.back().avg_height > detection_range_z_max_) { + pd_curr.point_state = PointLabel::OUT_OF_RANGE; continue; } + + // 2: continuously non-ground float points_xy_distance_square = - (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + - (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); + (point_curr.x - point_prev.x) * (point_curr.x - point_prev.x) + + (point_curr.y - point_prev.y) * (point_curr.y - point_prev.y); if ( - prev_p->point_state == PointLabel::NON_GROUND && + pd_prev.point_state == PointLabel::NON_GROUND && points_xy_distance_square < split_points_distance_tolerance_square_ && - p_orig_point.z > prev_p_orig_point.z) { - p->point_state = PointLabel::NON_GROUND; - out_no_ground_indices.indices.push_back(p->orig_index); + point_curr.z > point_prev.z) { + pd_curr.point_state = PointLabel::NON_GROUND; + out_no_ground_indices.indices.push_back(pd_curr.orig_index); continue; } + + // 3: the angle is exceed the global slope threshold if (global_slope_ratio_p > global_slope_max_ratio_) { - out_no_ground_indices.indices.push_back(p->orig_index); + out_no_ground_indices.indices.push_back(pd_curr.orig_index); continue; } - // gnd grid is continuous, the last gnd grid is close + uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; - float curr_grid_size = calcGridSize(*p); + float curr_grid_size = grid_.getGridSize(pd_curr.radius, pd_curr.grid_id); if ( - p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { - checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + // 4: the point is continuous with the previous grid + pd_curr.grid_id < next_gnd_grid_id_thresh && + pd_curr.radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(pd_curr, point_curr, gnd_grids); } else if ( - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { - checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); + // 5: the point is discontinuous with the previous grid + pd_curr.radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(pd_curr, point_curr, gnd_grids); } else { - checkBreakGndGrid(*p, p_orig_point, gnd_grids); + // 6: the point is break the previous grid + checkBreakGndGrid(pd_curr, point_curr, gnd_grids); } - if (p->point_state == PointLabel::NON_GROUND) { - out_no_ground_indices.indices.push_back(p->orig_index); - } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); + + // update the point label and update the ground cluster + if (pd_curr.point_state == PointLabel::NON_GROUND) { + out_no_ground_indices.indices.push_back(pd_curr.orig_index); + } else if (pd_curr.point_state == PointLabel::GROUND) { + centroid_bin.addPoint(pd_curr.radius, point_curr.z, pd_curr.orig_index); } + // else, the point is not classified } } } void ScanGroundFilterComponent::classifyPointCloud( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices) + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -505,24 +519,32 @@ void ScanGroundFilterComponent::classifyPointCloud( pcl::PointXYZ virtual_ground_point(0, 0, 0); calcVirtualGroundOrigin(virtual_ground_point); - // point classification algorithm - // sweep through each radial division + // run the classification algorithm for each ray (azimuth division) for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { float prev_gnd_radius = 0.0f; float prev_gnd_slope = 0.0f; PointsCentroid ground_cluster, non_ground_cluster; - PointLabel prev_point_label = PointLabel::INIT; - pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; + PointLabel point_label_curr = PointLabel::INIT; + + pcl::PointXYZ prev_gnd_point(0, 0, 0), point_curr, point_prev; - // loop through each point in the radial div + // iterate over the points in the ray for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { float points_distance = 0.0f; const float local_slope_max_angle = local_slope_max_angle_rad_; - prev_p_orig_point = p_orig_point; - auto * p = &in_radial_ordered_clouds[i][j]; - get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); + + // set the previous point + point_prev = point_curr; + PointLabel point_label_prev = point_label_curr; + + // set the current point + const PointData & pd = in_radial_ordered_clouds[i][j]; + point_label_curr = pd.point_state; + + const size_t data_index = in_cloud->point_step * pd.orig_index; + get_point_from_data_index(in_cloud, data_index, point_curr); if (j == 0) { - bool is_front_side = (p_orig_point.x > virtual_ground_point.x); + bool is_front_side = (point_curr.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -532,78 +554,78 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(p_orig_point, prev_gnd_point); + points_distance = calcDistance3d(point_curr, prev_gnd_point); } else { - points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); + points_distance = calcDistance3d(point_curr, point_prev); } - float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p_orig_point.z - prev_gnd_point.z; - float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); + float radius_distance_from_gnd = pd.radius - prev_gnd_radius; + float height_from_gnd = point_curr.z - prev_gnd_point.z; + float height_from_obj = point_curr.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < - (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); + (pd.radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope_ratio = p_orig_point.z / p->radius; + float global_slope_ratio = point_curr.z / pd.radius; // check points which is far enough from previous point if (global_slope_ratio > global_slope_max_ratio_) { - p->point_state = PointLabel::NON_GROUND; + point_label_curr = PointLabel::NON_GROUND; calculate_slope = false; } else if ( - (prev_point_label == PointLabel::NON_GROUND) && + (point_label_prev == PointLabel::NON_GROUND) && (std::abs(height_from_obj) >= split_height_distance_)) { calculate_slope = true; } else if (is_point_close_to_prev && std::abs(height_from_gnd) < split_height_distance_) { // close to the previous point, set point follow label - p->point_state = PointLabel::POINT_FOLLOW; + point_label_curr = PointLabel::POINT_FOLLOW; calculate_slope = false; } else { calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); - radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); + height_from_gnd = point_curr.z - ground_cluster.getAverageHeight(); + radius_distance_from_gnd = pd.radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { // far from the previous point auto local_slope = std::atan2(height_from_gnd, radius_distance_from_gnd); if (local_slope - prev_gnd_slope > local_slope_max_angle) { // the point is outside of the local slope threshold - p->point_state = PointLabel::NON_GROUND; + point_label_curr = PointLabel::NON_GROUND; } else { - p->point_state = PointLabel::GROUND; + point_label_curr = PointLabel::GROUND; } } - if (p->point_state == PointLabel::GROUND) { + if (point_label_curr == PointLabel::GROUND) { ground_cluster.initialize(); non_ground_cluster.initialize(); } - if (p->point_state == PointLabel::NON_GROUND) { - out_no_ground_indices.indices.push_back(p->orig_index); + if (point_label_curr == PointLabel::NON_GROUND) { + out_no_ground_indices.indices.push_back(pd.orig_index); } else if ( // NOLINT - (prev_point_label == PointLabel::NON_GROUND) && - (p->point_state == PointLabel::POINT_FOLLOW)) { - p->point_state = PointLabel::NON_GROUND; - out_no_ground_indices.indices.push_back(p->orig_index); + (point_label_prev == PointLabel::NON_GROUND) && + (point_label_curr == PointLabel::POINT_FOLLOW)) { + point_label_curr = PointLabel::NON_GROUND; + out_no_ground_indices.indices.push_back(pd.orig_index); } else if ( // NOLINT - (prev_point_label == PointLabel::GROUND) && (p->point_state == PointLabel::POINT_FOLLOW)) { - p->point_state = PointLabel::GROUND; + (point_label_prev == PointLabel::GROUND) && + (point_label_curr == PointLabel::POINT_FOLLOW)) { + point_label_curr = PointLabel::GROUND; } else { } // update the ground state - prev_point_label = p->point_state; - if (p->point_state == PointLabel::GROUND) { - prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); - ground_cluster.addPoint(p->radius, p_orig_point.z); + if (point_label_curr == PointLabel::GROUND) { + prev_gnd_radius = pd.radius; + prev_gnd_point = pcl::PointXYZ(point_curr.x, point_curr.y, point_curr.z); + ground_cluster.addPoint(pd.radius, point_curr.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state - if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p_orig_point.z); + if (point_label_curr == PointLabel::NON_GROUND) { + non_ground_cluster.addPoint(pd.radius, point_curr.z); } } } @@ -611,7 +633,7 @@ void ScanGroundFilterComponent::classifyPointCloud( void ScanGroundFilterComponent::extractObjectPoints( const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, - PointCloud2 & out_object_cloud) + PointCloud2 & out_object_cloud) const { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -637,8 +659,8 @@ void ScanGroundFilterComponent::faster_filter( std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - if (!offset_initialized_) { - set_field_offsets(input); + if (!data_offset_initialized_) { + set_field_index_offsets(input); } std::vector radial_ordered_points; @@ -684,37 +706,16 @@ void ScanGroundFilterComponent::filter( } rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( - const std::vector & p) + const std::vector & param) { - if (get_param(p, "grid_size_m", grid_size_m_)) { - grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - tan_grid_size_rad_ = std::tan(grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); - RCLCPP_DEBUG( - get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); - RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + if (get_param(param, "grid_size_m", grid_size_m_)) { + grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); } - if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { - grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; - grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - tan_grid_size_rad_ = std::tan(grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); - RCLCPP_DEBUG( - get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); - RCLCPP_DEBUG( - get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); - RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); - RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + if (get_param(param, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_.initialize(grid_size_m_, grid_mode_switch_radius_, virtual_lidar_z_); } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; - if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { + if (get_param(param, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( @@ -722,7 +723,7 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; - if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { + if (get_param(param, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( @@ -730,14 +731,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; - if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { + if (get_param(param, "radial_divider_angle_deg", radial_divider_angle_deg)) { radial_divider_angle_rad_ = deg2rad(radial_divider_angle_deg); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting radial_divider_angle_rad to: %f.", radial_divider_angle_rad_); RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } - if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + if (get_param(param, "split_points_distance_tolerance", split_points_distance_tolerance_)) { split_points_distance_tolerance_square_ = split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( @@ -747,15 +748,15 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( get_logger(), "Setting split_points_distance_tolerance_square to: %f.", split_points_distance_tolerance_square_); } - if (get_param(p, "split_height_distance", split_height_distance_)) { + if (get_param(param, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); } - if (get_param(p, "use_virtual_ground_point", use_virtual_ground_point_)) { + if (get_param(param, "use_virtual_ground_point", use_virtual_ground_point_)) { RCLCPP_DEBUG_STREAM( get_logger(), "Setting use_virtual_ground_point to: " << std::boolalpha << use_virtual_ground_point_); } - if (get_param(p, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) { + if (get_param(param, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) { RCLCPP_DEBUG_STREAM( get_logger(), "Setting use_recheck_ground_cluster to: " << std::boolalpha << use_recheck_ground_cluster_); diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index 12590afd5fb86..bccef138e754e 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -19,6 +19,7 @@ #include "autoware/pointcloud_preprocessor/transform_info.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info.hpp" +#include "grid.hpp" #include @@ -134,7 +135,7 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt addPoint(radius, height); } - float getAverageSlope() { return std::atan2(height_avg, radius_avg); } + float getAverageSlope() const { return std::atan2(height_avg, radius_avg); } float getAverageHeight() const { return height_avg; } @@ -144,8 +145,8 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float getMinHeight() const { return height_min; } - pcl::PointIndices & getIndicesRef() { return pcl_indices; } - std::vector & getHeightListRef() { return height_list; } + const pcl::PointIndices & getIndicesRef() const { return pcl_indices; } + const std::vector & getHeightListRef() const { return height_list; } }; void filter( @@ -160,47 +161,56 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; - int x_offset_; - int y_offset_; - int z_offset_; - int intensity_offset_; + int data_offset_x_; + int data_offset_y_; + int data_offset_z_; + int data_offset_intensity_; int intensity_type_; - bool offset_initialized_; - - void set_field_offsets(const PointCloud2ConstPtr & input); - - void get_point_from_global_offset( - const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + bool data_offset_initialized_; const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; - float grid_size_rad_; - float tan_grid_size_rad_; - float grid_size_m_; float low_priority_region_x_; - uint16_t gnd_grid_buffer_size_; - float grid_mode_switch_grid_id_; - float grid_mode_switch_angle_rad_; - float virtual_lidar_z_; - float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance + float center_pcl_shift_; // virtual center of pcl to center mass + + // common parameters + double radial_divider_angle_rad_; // distance in rads between dividers + size_t radial_dividers_num_; + VehicleInfo vehicle_info_; + + // common thresholds double global_slope_max_angle_rad_; // radians double local_slope_max_angle_rad_; // radians double global_slope_max_ratio_; double local_slope_max_ratio_; - double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions double split_points_distance_tolerance_square_; + + // non-grid mode parameters + bool use_virtual_ground_point_; double // minimum height threshold regardless the slope, split_height_distance_; // useful for close points - bool use_virtual_ground_point_; + + // grid mode parameters bool use_recheck_ground_cluster_; // to enable recheck ground cluster bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster, // otherwise select middle point - size_t radial_dividers_num_; - VehicleInfo vehicle_info_; + float detection_range_z_max_; + + // grid parameters + float grid_size_m_; + float grid_mode_switch_radius_; // non linear grid size switching distance + uint16_t gnd_grid_buffer_size_; + float virtual_lidar_z_; + + // grid data + ScanGroundGrid grid_; + + // data access methods + void set_field_index_offsets(const PointCloud2ConstPtr & input); + void get_point_from_data_index( + const PointCloud2ConstPtr & input, const size_t data_index, pcl::PointXYZ & point) const; // time keeper related rclcpp::Publisher::SharedPtr @@ -224,17 +234,15 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt */ void convertPointcloud( const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points); + std::vector & out_radial_ordered_points) const; void convertPointcloudGridScan( const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points); + std::vector & out_radial_ordered_points) const; /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ - void calcVirtualGroundOrigin(pcl::PointXYZ & point); - - float calcGridSize(const PointData & p); + void calcVirtualGroundOrigin(pcl::PointXYZ & point) const; /*! * Classifies Points in the PointCloud as Ground and Not Ground @@ -245,23 +253,25 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt */ void initializeFirstGndGrids( - const float h, const float r, const uint16_t id, std::vector & gnd_grids); + const float h, const float r, const uint16_t id, std::vector & gnd_grids) const; void checkContinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, - const std::vector & gnd_grids_list); + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const; void checkDiscontinuousGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, - const std::vector & gnd_grids_list); + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const; void checkBreakGndGrid( - PointData & p, const pcl::PointXYZ & p_orig_point, - const std::vector & gnd_grids_list); + PointData & pd, const pcl::PointXYZ & point_curr, + const std::vector & gnd_grids_list) const; void classifyPointCloud( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices); + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const; void classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, - pcl::PointIndices & out_no_ground_indices); + const PointCloud2ConstPtr & in_cloud, + const std::vector & in_radial_ordered_clouds, + pcl::PointIndices & out_no_ground_indices) const; /*! * Re-classifies point of ground cluster based on their height * @param gnd_cluster Input ground cluster for re-checking @@ -269,8 +279,8 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt * @param non_ground_indices Output non-ground PointCloud indices */ void recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, - pcl::PointIndices & non_ground_indices); + const PointsCentroid & gnd_cluster, const float non_ground_threshold, + const bool use_lowest_point, pcl::PointIndices & non_ground_indices) const; /*! * Returns the resulting complementary PointCloud, one with the points kept * and the other removed as indicated in the indices @@ -280,13 +290,14 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt */ void extractObjectPoints( const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, - PointCloud2 & out_object_cloud); + PointCloud2 & out_object_cloud) const; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult onParameter(const std::vector & p); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & param); // debugger std::unique_ptr> stop_watch_ptr_{ From 146be20c760d6b1d9b8023dd5cd96ad2ab89917e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 10 Oct 2024 13:35:42 +0900 Subject: [PATCH 05/27] refactor(goal_planner): remove lane parking pull over path (#9063) Signed-off-by: Mamoru Sobue --- .../pull_over_planner_base.hpp | 6 +++--- .../thread_data.hpp | 8 ------- .../src/goal_planner_module.cpp | 21 +++++++++++++------ 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 2a328a467723c..e061a0203602f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -55,7 +55,7 @@ struct PullOverPath size_t id() const { return id_; } Pose start_pose() const { return start_pose_; } Pose modified_goal_pose() const { return modified_goal_pose_.goal_pose; } - GoalCandidate modified_goal() const { return modified_goal_pose_; } + const GoalCandidate & modified_goal() const { return modified_goal_pose_; } std::vector & partial_paths() { return partial_paths_; } const std::vector & partial_paths() const { return partial_paths_; } @@ -63,8 +63,8 @@ struct PullOverPath // TODO(soblin): use reference to avoid copy once thread-safe design is finished const PathWithLaneId & full_path() const { return full_path_; } PathWithLaneId parking_path() const { return parking_path_; } - std::vector full_path_curvatures() { return full_path_curvatures_; } - std::vector parking_path_curvatures() const { return parking_path_curvatures_; } + const std::vector & full_path_curvatures() const { return full_path_curvatures_; } + const std::vector & parking_path_curvatures() const { return parking_path_curvatures_; } double full_path_max_curvature() const { return full_path_max_curvature_; } double parking_path_max_curvature() const { return parking_path_max_curvature_; } size_t path_idx() const { return path_idx_; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index 2d3dd30bdf7ef..3991ae0f7dacd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -119,7 +119,6 @@ class ThreadSafeData } DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) @@ -137,9 +136,6 @@ class ThreadSafeData void set_pull_over_path_no_lock(const PullOverPath & path) { pull_over_path_ = std::make_shared(path); - if (path.type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } last_path_update_time_ = clock_->now(); } @@ -147,9 +143,6 @@ class ThreadSafeData void set_pull_over_path_no_lock(const std::shared_ptr & path) { pull_over_path_ = path; - if (path->type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } last_path_update_time_ = clock_->now(); } @@ -164,7 +157,6 @@ class ThreadSafeData } std::shared_ptr pull_over_path_{nullptr}; - std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; GoalCandidates goal_candidates_{}; std::optional last_path_update_time_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 03d5dfdcf2a24..4c53425d525f2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -17,6 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -856,13 +857,16 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return false; } - const auto lane_parking_path = thread_safe_data_.get_lane_parking_pull_over_path(); - if (!lane_parking_path) { + if (!context_data.pull_over_path_opt) { + return false; + } + if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { return false; } + const auto & lane_parking_path = context_data.pull_over_path_opt.value(); - const PathWithLaneId path = lane_parking_path->full_path(); - const std::vector curvatures = lane_parking_path->full_path_curvatures(); + const auto & path = lane_parking_path.full_path(); + const auto & curvatures = lane_parking_path.full_path_curvatures(); if ( parameters_->use_object_recognition && goal_planner_utils::checkObjectsCollision( @@ -1438,7 +1442,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // return to lane parking if it is possible if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) { - thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); + // TODO(soblin): return from freespace to lane is disabled temporarily, because if + // context_data_with_velocity contained freespace path, since lane_parking_pull_over_path is + // deleted, freespace path is set again + if (context_data_with_velocity.pull_over_path_opt) { + thread_safe_data_.set_pull_over_path(context_data_with_velocity.pull_over_path_opt.value()); + } } // For debug @@ -2728,7 +2737,7 @@ PathDecisionState PathDecisionStateController::get_next_state( // check current parking path collision const auto & parking_path = pull_over_path.parking_path(); - const std::vector parking_path_curvatures = pull_over_path.parking_path_curvatures(); + const auto & parking_path_curvatures = pull_over_path.parking_path_curvatures(); const double margin = parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; if (goal_planner_utils::checkObjectsCollision( From 23bb4a0ccf50c11fe9db7c27cf3844a9c4904dcc Mon Sep 17 00:00:00 2001 From: Samrat Thapa <38401989+SamratThapa120@users.noreply.github.com> Date: Thu, 10 Oct 2024 13:45:28 +0900 Subject: [PATCH 06/27] fix(autoware_lidar_transfusion): set tensor names by matching with predefined values. (#9057) * set tensor order using api Signed-off-by: Samrat Thapa * style(pre-commit): autofix Signed-off-by: Samrat Thapa * fix tensor order Signed-off-by: Samrat Thapa * style(pre-commit): autofix Signed-off-by: Samrat Thapa * style fix Signed-off-by: Samrat Thapa * style(pre-commit): autofix --------- Signed-off-by: Samrat Thapa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware/lidar_transfusion/utils.hpp | 2 +- .../lib/network/network_trt.cpp | 22 ++++++++++++++++++- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp index feea2216c5f78..f3a7cecf9ce00 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp @@ -36,7 +36,7 @@ struct Box3D float yaw; }; -enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; +enum NetworkIO { voxels = 0, num_points, coors, cls_score, bbox_pred, dir_pred, ENUM_SIZE }; // cspell: ignore divup template diff --git a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp index 3eacf005cff4e..48e8c1014199c 100644 --- a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp @@ -23,6 +23,20 @@ namespace autoware::lidar_transfusion { +inline NetworkIO nameToNetworkIO(const char * name) +{ + static const std::unordered_map name_to_enum = { + {"voxels", NetworkIO::voxels}, {"num_points", NetworkIO::num_points}, + {"coors", NetworkIO::coors}, {"cls_score0", NetworkIO::cls_score}, + {"bbox_pred0", NetworkIO::bbox_pred}, {"dir_cls_pred0", NetworkIO::dir_pred}}; + + auto it = name_to_enum.find(name); + if (it != name_to_enum.end()) { + return it->second; + } + throw std::runtime_error("Invalid input name: " + std::string(name)); +} + std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) { std::string delim = ""; @@ -253,8 +267,14 @@ bool NetworkTRT::validateNetworkIO() << ". Actual size: " << engine->getNbIOTensors() << "." << std::endl; throw std::runtime_error("Failed to initialize TRT network."); } + + // Initialize tensors_names_ with null values + tensors_names_.resize(NetworkIO::ENUM_SIZE, nullptr); + + // Loop over the tensor names and place them in the correct positions for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { - tensors_names_.push_back(engine->getIOTensorName(i)); + const char * name = engine->getIOTensorName(i); + tensors_names_[nameToNetworkIO(name)] = name; } // Log the network IO From 2ab8f882a4c2cdd6694a57c1b52fc8cb5becfea8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 10 Oct 2024 14:02:08 +0900 Subject: [PATCH 07/27] refactor(goal_planner): remove prev_data / last_path_idx_time from ThreadSafeData (#9064) refactor(goal_planner): remove prev_data and last_path_idx_update_time Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 15 +++++-- .../thread_data.hpp | 27 ++++++------- .../src/goal_planner_module.cpp | 40 +++++++++---------- 3 files changed, 44 insertions(+), 38 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 93fc38f261117..e8079c4e27720 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" @@ -111,12 +112,15 @@ struct PullOverContextData explicit PullOverContextData( const bool is_stable_safe_path, const PredictedObjects & static_objects, const PredictedObjects & dynamic_objects, std::optional && pull_over_path_opt, - const std::vector & pull_over_path_candidates) + const std::vector & pull_over_path_candidates, + const PathDecisionState & prev_state) : is_stable_safe_path(is_stable_safe_path), static_target_objects(static_objects), dynamic_target_objects(dynamic_objects), pull_over_path_opt(pull_over_path_opt), - pull_over_path_candidates(pull_over_path_candidates) + pull_over_path_candidates(pull_over_path_candidates), + prev_state_for_debug(prev_state), + last_path_idx_increment_time(std::nullopt) { } const bool is_stable_safe_path; @@ -126,6 +130,8 @@ struct PullOverContextData std::optional pull_over_path_opt; const std::vector pull_over_path_candidates; // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose + const PathDecisionState prev_state_for_debug; + std::optional last_path_idx_increment_time; }; class GoalPlannerModule : public SceneModuleInterface @@ -364,7 +370,8 @@ class GoalPlannerModule : public SceneModuleInterface PathWithLaneId generateStopPath(const PullOverContextData & context_data) const; PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; - void keepStoppedWithCurrentPath(PathWithLaneId & path) const; + void keepStoppedWithCurrentPath( + const PullOverContextData & ctx_data, PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status @@ -464,7 +471,7 @@ class GoalPlannerModule : public SceneModuleInterface */ bool isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index 3991ae0f7dacd..3b526bf3a8f24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -63,11 +63,7 @@ class ThreadSafeData return false; } - if (pull_over_path_->incrementPathIndex()) { - last_path_idx_increment_time_ = clock_->now(); - return true; - } - return false; + return pull_over_path_->incrementPathIndex(); } void set_pull_over_path(const PullOverPath & path) @@ -112,25 +108,29 @@ class ThreadSafeData pull_over_path_candidates_.clear(); goal_candidates_.clear(); last_path_update_time_ = std::nullopt; - last_path_idx_increment_time_ = std::nullopt; closest_start_pose_ = std::nullopt; last_previous_module_output_ = std::nullopt; prev_data_ = PathDecisionState{}; } + // main --> lane/freespace + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + // main --> lane + DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) + + // lane --> main + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + + // main <--> lane/freespace DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) DEFINE_SETTER_GETTER_WITH_MUTEX( utils::path_safety_checker::CollisionCheckDebugMap, collision_check) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) private: void set_pull_over_path_no_lock(const PullOverPath & path) @@ -160,7 +160,6 @@ class ThreadSafeData std::vector pull_over_path_candidates_; GoalCandidates goal_candidates_{}; std::optional last_path_update_time_; - std::optional last_path_idx_increment_time_; std::optional closest_start_pose_{}; std::optional last_previous_module_output_{}; utils::path_safety_checker::CollisionCheckDebugMap collision_check_{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 4c53425d525f2..88349f20a0540 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -572,9 +572,12 @@ void GoalPlannerModule::updateData() return pull_over_path.modified_goal(); }(); + // save "old" state + const auto prev_decision_state = path_decision_controller_.get_current_state(); const bool is_current_safe = isSafePath( - planner_data_, found_pull_over_path, pull_over_path_recv, *parameters_, + planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); + // update to latest state path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, @@ -583,8 +586,8 @@ void GoalPlannerModule::updateData() context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, dynamic_target_objects, std::move(pull_over_path_recv), - thread_safe_data_.get_pull_over_path_candidates()); - const auto & ctx_data = context_data_.value(); + thread_safe_data_.get_pull_over_path_candidates(), prev_decision_state); + auto & ctx_data = context_data_.value(); // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { @@ -600,7 +603,9 @@ void GoalPlannerModule::updateData() } if (hasFinishedCurrentPath(ctx_data)) { - thread_safe_data_.incrementPathIndex(); + if (thread_safe_data_.incrementPathIndex()) { + ctx_data.last_path_idx_increment_time = clock_->now(); + } } if (!last_approval_data_) { @@ -1032,7 +1037,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // Create a map of PullOverPath pointer to largest collision check margin std::map path_id_to_rough_margin_map; - const auto target_objects = thread_safe_data_.get_static_target_objects(); + const auto & target_objects = context_data.static_target_objects; for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( @@ -1212,7 +1217,7 @@ void GoalPlannerModule::setOutput( // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected auto current_path = pull_over_path.getCurrentPath(); - keepStoppedWithCurrentPath(current_path); + keepStoppedWithCurrentPath(context_data, current_path); output.path = current_path; } @@ -1471,7 +1476,7 @@ void GoalPlannerModule::postProcess() RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); } const auto context_data_dummy = - PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}); + PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}, {}); const auto & context_data = context_data_.has_value() ? context_data_.value() : context_data_dummy; @@ -1957,15 +1962,16 @@ bool GoalPlannerModule::hasEnoughDistance( return true; } -void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) const +void GoalPlannerModule::keepStoppedWithCurrentPath( + const PullOverContextData & ctx_data, PathWithLaneId & path) const { + const auto last_path_idx_increment_time = ctx_data.last_path_idx_increment_time; constexpr double keep_stop_time = 2.0; - if (!thread_safe_data_.get_last_path_idx_increment_time()) { + if (!last_path_idx_increment_time) { return; } - const auto time_diff = - (clock_->now() - *thread_safe_data_.get_last_path_idx_increment_time()).seconds(); + const auto time_diff = (clock_->now() - last_path_idx_increment_time.value()).seconds(); if (time_diff > keep_stop_time) { return; } @@ -2225,7 +2231,8 @@ static std::vector filterOb bool GoalPlannerModule::isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, const GoalPlannerParameters & parameters, + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, + const GoalPlannerParameters & parameters, const std::shared_ptr & ego_predicted_path_params, const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const @@ -2312,7 +2319,6 @@ bool GoalPlannerModule::isSafePath( const auto filtered_objects = filterObjectsByWithinPolicy( dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params); - const auto prev_data = thread_safe_data_.get_prev_data(); const double hysteresis_factor = prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; @@ -2417,12 +2423,6 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) add(createPathMarkerArray( getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); - const auto last_previous_module_output = thread_safe_data_.get_last_previous_module_output(); - if (last_previous_module_output.has_value()) { - add(createPathMarkerArray( - last_previous_module_output.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); - } - // Visualize path and related pose if (context_data.pull_over_path_opt) { const auto & pull_over_path = context_data.pull_over_path_opt.value(); @@ -2506,7 +2506,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // visualize safety status maker { - const auto prev_data = thread_safe_data_.get_prev_data(); + const auto & prev_data = context_data.prev_state_for_debug; visualization_msgs::msg::MarkerArray marker_array{}; const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) : createMarkerColor(1.0, 0.0, 0.0, 0.99); From 17467985414019c0861505c7b5a77ae427ae3977 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=81=90=E3=82=8B=E3=81=90=E3=82=8B?= Date: Thu, 10 Oct 2024 14:42:02 +0900 Subject: [PATCH 08/27] fix: fix Boost.Optional build error on Jazzy (#7602) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kotaro Yoshimoto --- .../test/src/lowpass_filter_1d_test.cpp | 6 ++++-- .../test/src/lowpass_filter_test.cpp | 6 ++++-- .../test/src/test_predicted_path_utils.cpp | 12 +++++++----- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp index 44e8a1ffce437..ba43219f325ff 100644 --- a/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -14,6 +14,8 @@ #include "autoware/signal_processing/lowpass_filter_1d.hpp" +#include + #include constexpr double epsilon = 1e-6; @@ -25,7 +27,7 @@ TEST(lowpass_filter_1d, filter) LowpassFilter1d lowpass_filter_1d(0.1); // initial state - EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_1d.getValue()); // random filter EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); @@ -35,7 +37,7 @@ TEST(lowpass_filter_1d, filter) // reset without value lowpass_filter_1d.reset(); - EXPECT_EQ(lowpass_filter_1d.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_1d.getValue()); // reset with value lowpass_filter_1d.reset(-1.1); diff --git a/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp index 864378719dd5c..83ccc0f5dab03 100644 --- a/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp +++ b/common/autoware_signal_processing/test/src/lowpass_filter_test.cpp @@ -14,6 +14,8 @@ #include "autoware/signal_processing/lowpass_filter.hpp" +#include + #include constexpr double epsilon = 1e-6; @@ -41,7 +43,7 @@ TEST(lowpass_filter_twist, filter) LowpassFilterTwist lowpass_filter_(0.1); { // initial state - EXPECT_EQ(lowpass_filter_.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_.getValue()); } { // random filter @@ -58,7 +60,7 @@ TEST(lowpass_filter_twist, filter) { // reset without value lowpass_filter_.reset(); - EXPECT_EQ(lowpass_filter_.getValue(), boost::none); + EXPECT_FALSE(lowpass_filter_.getValue()); } { // reset with value diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index 305a1173acf12..228a0d097fde6 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -16,6 +16,8 @@ #include "autoware/universe_utils/math/unit_conversion.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include + #include using autoware::universe_utils::Point2d; @@ -75,7 +77,7 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) for (double t = 0.0; t < 9.0 + 1e-6; t += 1.0) { const auto p = calcInterpolatedPose(path, t); - EXPECT_NE(p, boost::none); + EXPECT_TRUE(p); EXPECT_NEAR(p->position.x, t * 1.0, epsilon); EXPECT_NEAR(p->position.y, 0.0, epsilon); EXPECT_NEAR(p->position.z, 0.0, epsilon); @@ -92,7 +94,7 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) for (double t = 0.0; t < 9.0; t += 0.3) { const auto p = calcInterpolatedPose(path, t); - EXPECT_NE(p, boost::none); + EXPECT_TRUE(p); EXPECT_NEAR(p->position.x, t * 1.0, epsilon); EXPECT_NEAR(p->position.y, 0.0, epsilon); EXPECT_NEAR(p->position.z, 0.0, epsilon); @@ -108,20 +110,20 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) // Negative time { const auto p = calcInterpolatedPose(path, -1.0); - EXPECT_EQ(p, boost::none); + EXPECT_FALSE(p); } // Over the time horizon { const auto p = calcInterpolatedPose(path, 11.0); - EXPECT_EQ(p, boost::none); + EXPECT_FALSE(p); } // Empty Path { PredictedPath empty_path; const auto p = calcInterpolatedPose(empty_path, 5.0); - EXPECT_EQ(p, boost::none); + EXPECT_FALSE(p); } } } From b93ddd96aede44ec8bc66b766ea939747b866f69 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Thu, 10 Oct 2024 16:42:45 +0900 Subject: [PATCH 09/27] test(bpp_common): add test for occupancy grid based collision detector (#9066) * add test for occupancy grid based collision detector Signed-off-by: Go Sakayori * remove unnnecessary header Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori * change map resolution and corresponding index Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- .../CMakeLists.txt | 8 + ...ccupancy_grid_based_collision_detector.hpp | 86 ++++++---- ...ccupancy_grid_based_collision_detector.cpp | 51 ++---- ...ccupancy_grid_based_collision_detector.cpp | 149 ++++++++++++++++++ 4 files changed, 226 insertions(+), 68 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index 046675f357923..e24abb51de6fc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -50,6 +50,14 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_occupancy_grid_based_collision_detector + test/test_occupancy_grid_based_collision_detector.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_occupancy_grid_based_collision_detector + ${PROJECT_NAME} + ) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check test/test_safety_check.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index fa04ff04c8e44..434eb79189eaf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -25,7 +25,13 @@ namespace autoware::behavior_path_planner { -int discretizeAngle(const double theta, const int theta_size); +/** + * @brief Discretizes a given angle into an index within a specified range. + * @param theta The angle in radians to discretize. + * @param theta_size The number of discrete bins or angular intervals. + * @return The discretized angle as an integer index. + */ +int discretize_angle(const double theta, const int theta_size); struct IndexXYT { @@ -40,19 +46,36 @@ struct IndexXY int y; }; +/** + * @brief Converts a given local pose into a 3D grid index (x, y, theta). + * @param costmap The occupancy grid used for indexing. + * @param pose_local The local pose. + * @param theta_size The number of discrete angular intervals for yaw. + * @return IndexXYT The grid index representing the position and discretized orientation. + */ IndexXYT pose2index( const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local, const int theta_size); +/** + * @brief Converts a grid index (x, y, theta) back into a pose in the local frame. + * @param costmap The occupancy grid used for indexing. + * @param index The grid index containing x, y, and theta. + * @param theta_size The number of discrete angular intervals for yaw. + * @return geometry_msgs::msg::Pose The corresponding local pose. + */ geometry_msgs::msg::Pose index2pose( const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size); +/** + * @brief Transforms a global pose into a local pose relative to the costmap's origin. + * @param costmap The occupancy grid that defines the local frame. + * @param pose_global The global pose to transform. + * @return geometry_msgs::msg::Pose The transformed pose in the local frame. + */ geometry_msgs::msg::Pose global2local( const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global); -geometry_msgs::msg::Pose local2global( - const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local); - struct VehicleShape { double length; // X [m] @@ -76,12 +99,6 @@ struct PlannerWaypoint bool is_back = false; }; -struct PlannerWaypoints -{ - std_msgs::msg::Header header; - std::vector waypoints; -}; - class OccupancyGridBasedCollisionDetector { public: @@ -92,21 +109,40 @@ class OccupancyGridBasedCollisionDetector default; OccupancyGridBasedCollisionDetector & operator=(OccupancyGridBasedCollisionDetector &&) = delete; void setParam(const OccupancyGridMapParam & param) { param_ = param; }; - OccupancyGridMapParam getParam() const { return param_; }; + [[nodiscard]] OccupancyGridMapParam getParam() const { return param_; }; void setMap(const nav_msgs::msg::OccupancyGrid & costmap); - nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; }; - void setVehicleShape(const VehicleShape & vehicle_shape) { param_.vehicle_shape = vehicle_shape; } - bool hasObstacleOnPath( - const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const; - bool hasObstacleOnPath( + [[nodiscard]] nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; }; + + /** + * @brief Detects if a collision occurs with given path. + * @param path The path to check collision defined in a global frame + * @param check_out_of_range A boolean flag indicating whether out-of-range conditions is + * collision + * @return true if a collision is detected, false if no collision is detected. + */ + [[nodiscard]] bool hasObstacleOnPath( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; - const PlannerWaypoints & getWaypoints() const { return waypoints_; } - bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; + + /** + * @brief Detects if a collision occurs at the specified base index in the occupancy grid map. + * @param base_index The 3D index (x, y, theta) of the base position in the occupancy grid. + * @param check_out_of_range A boolean flag indicating whether out-of-range conditions is + * collision + * @return true if a collision is detected, false if no collision is detected. + */ + [[nodiscard]] bool detectCollision( + const IndexXYT & base_index, const bool check_out_of_range) const; virtual ~OccupancyGridBasedCollisionDetector() = default; protected: - void computeCollisionIndexes(int theta_index, std::vector & indexes); - inline bool isOutOfRange(const IndexXYT & index) const + /** + * @brief Computes the 2D grid indexes where collision might occur for a given theta index. + * @param theta_index The discretized orientation index for yaw. + * @param indexes_2d The output vector of 2D grid indexes where collisions could happen. + */ + void compute_collision_indexes(int theta_index, std::vector & indexes); + + [[nodiscard]] inline bool is_out_of_range(const IndexXYT & index) const { if (index.x < 0 || static_cast(costmap_.info.width) <= index.x) { return true; @@ -116,7 +152,7 @@ class OccupancyGridBasedCollisionDetector } return false; } - inline bool isObs(const IndexXYT & index) const + [[nodiscard]] inline bool is_obs(const IndexXYT & index) const { // NOTE: Accessing by .at() instead makes 1.2 times slower here. // Also, boundary check is already done in isOutOfRange before calling this function. @@ -134,15 +170,7 @@ class OccupancyGridBasedCollisionDetector // is_obstacle's table std::vector> is_obstacle_table_; - - // pose in costmap frame - geometry_msgs::msg::Pose start_pose_; - geometry_msgs::msg::Pose goal_pose_; - - // result path - PlannerWaypoints waypoints_; }; - } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index e4f1919b08dbc..e65c67065bc54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -25,7 +25,7 @@ using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::normalizeRadian; using autoware::universe_utils::transformPose; -int discretizeAngle(const double theta, const int theta_size) +int discretize_angle(const double theta, const int theta_size) { const double one_angle_range = 2.0 * M_PI / theta_size; return static_cast(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size; @@ -37,7 +37,7 @@ IndexXYT pose2index( { const int index_x = pose_local.position.x / costmap.info.resolution; const int index_y = pose_local.position.y / costmap.info.resolution; - const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size); + const int index_theta = discretize_angle(tf2::getYaw(pose_local.orientation), theta_size); return {index_x, index_y, index_theta}; } @@ -68,18 +68,6 @@ geometry_msgs::msg::Pose global2local( return transformPose(pose_global, transform); } -geometry_msgs::msg::Pose local2global( - const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local) -{ - tf2::Transform tf_origin; - tf2::convert(costmap.info.origin, tf_origin); - - geometry_msgs::msg::TransformStamped transform; - transform.transform = tf2::toMsg(tf_origin); - - return transformPose(pose_local, transform); -} - void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyGrid & costmap) { costmap_ = costmap; @@ -105,7 +93,7 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG coll_indexes_table_.clear(); for (int i = 0; i < param_.theta_size; i++) { std::vector indexes_2d; - computeCollisionIndexes(i, indexes_2d); + compute_collision_indexes(i, indexes_2d); coll_indexes_table_.push_back(indexes_2d); } } @@ -118,7 +106,7 @@ static IndexXY position2index( return {index_x, index_y}; } -void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( +void OccupancyGridBasedCollisionDetector::compute_collision_indexes( int theta_index, std::vector & indexes_2d) { IndexXYT base_index{0, 0, theta_index}; @@ -134,7 +122,7 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( const auto base_theta = tf2::getYaw(base_pose.orientation); // Convert each point to index and check if the node is Obstacle - const auto addIndex2d = [&](const double x, const double y) { + const auto add_index2d = [&](const double x, const double y) { // Calculate offset in rotated frame const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; @@ -149,14 +137,14 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( for (double x = back; x <= front; x += costmap_.info.resolution / 2) { for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(x, y); + add_index2d(x, y); } - addIndex2d(x, left); + add_index2d(x, left); } for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(front, y); + add_index2d(front, y); } - addIndex2d(front, left); + add_index2d(front, left); } bool OccupancyGridBasedCollisionDetector::detectCollision( @@ -175,27 +163,12 @@ bool OccupancyGridBasedCollisionDetector::detectCollision( coll_index.x += base_index.x; coll_index.y += base_index.y; if (check_out_of_range) { - if (isOutOfRange(coll_index) || isObs(coll_index)) return true; + if (is_out_of_range(coll_index) || is_obs(coll_index)) return true; } else { - if (isOutOfRange(coll_index)) return false; - if (isObs(coll_index)) return true; - } - } - return false; -} - -bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const -{ - for (const auto & pose : path.poses) { - const auto pose_local = global2local(costmap_, pose); - const auto index = pose2index(costmap_, pose_local, param_.theta_size); - - if (detectCollision(index, check_out_of_range)) { - return true; + if (is_out_of_range(coll_index)) return false; + if (is_obs(coll_index)) return true; } } - return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp new file mode 100644 index 0000000000000..b212cce551252 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_occupancy_grid_based_collision_detector.cpp @@ -0,0 +1,149 @@ +// 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 "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include + +#include + +using autoware::behavior_path_planner::OccupancyGridBasedCollisionDetector; +using autoware::behavior_path_planner::OccupancyGridMapParam; +using autoware::test_utils::createPose; + +class OccupancyGridBasedCollisionDetectorTest : public ::testing::Test +{ +protected: + OccupancyGridBasedCollisionDetector detector_; + nav_msgs::msg::OccupancyGrid costmap_; + OccupancyGridMapParam param_; + + void SetUp() override + { + costmap_.info.width = 40; + costmap_.info.height = 40; + costmap_.info.resolution = 0.25; + costmap_.data = std::vector(1600, 0); + costmap_.data[28 * costmap_.info.width + 28] = 100; + costmap_.info.origin = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); + + param_.theta_size = 8; + param_.obstacle_threshold = 1; + param_.vehicle_shape.base2back = 0.0; + param_.vehicle_shape.length = 4.0; + param_.vehicle_shape.width = 2.0; + detector_.setParam(param_); + } +}; + +TEST_F(OccupancyGridBasedCollisionDetectorTest, discretize_angle) +{ + using autoware::behavior_path_planner::discretize_angle; + + int theta_size = 8; + + EXPECT_EQ(discretize_angle(0.0, theta_size), 0); + EXPECT_EQ(discretize_angle(M_PI * 0.25, theta_size), 1); + EXPECT_EQ(discretize_angle(2.5 * M_PI, theta_size), 2); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, pose2index) +{ + using autoware::behavior_path_planner::pose2index; + + auto pose_local = createPose(2.0, 3.0, 0.0, 0.0, 0.0, 0.0); + + int theta_size = 8; + auto index = pose2index(costmap_, pose_local, theta_size); + + EXPECT_EQ(index.x, 8); + EXPECT_EQ(index.y, 12); + EXPECT_EQ(index.theta, 0); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, index2pose) +{ + using autoware::behavior_path_planner::index2pose; + + autoware::behavior_path_planner::IndexXYT index{4, 6, 2}; + + int theta_size = 8; + auto pose_local = index2pose(costmap_, index, theta_size); + + EXPECT_DOUBLE_EQ(pose_local.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose_local.position.y, 1.5); + EXPECT_DOUBLE_EQ(tf2::getYaw(pose_local.orientation), M_PI * 0.5); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, global2local) +{ + using autoware::behavior_path_planner::global2local; + + auto pose_global = createPose(2.0, 2.0, 0.0, 0.0, 0.0, 0.0); + auto pose_local = global2local(costmap_, pose_global); + + EXPECT_DOUBLE_EQ(pose_local.position.x, 1.0); + EXPECT_DOUBLE_EQ(pose_local.position.y, 1.0); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, detectCollision) +{ + using autoware::behavior_path_planner::IndexXYT; + + // Condition: map not set + IndexXYT base_index{0, 0, 0}; + base_index.x = 24; + base_index.y = 24; + EXPECT_FALSE(detector_.detectCollision(base_index, true)); + + // Condition: with object + detector_.setMap(costmap_); + EXPECT_TRUE(detector_.detectCollision(base_index, true)); + + // Condition: position without obstacle + base_index.x = 4; + base_index.y = 4; + EXPECT_FALSE(detector_.detectCollision(base_index, true)); + + // Condition: position out of range + base_index.x = -100; + base_index.y = -100; + EXPECT_TRUE(detector_.detectCollision(base_index, true)); + EXPECT_FALSE(detector_.detectCollision(base_index, false)); +} + +TEST_F(OccupancyGridBasedCollisionDetectorTest, hasObstacleOnPath) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + detector_.setMap(costmap_); + + // Condition: empty path + EXPECT_FALSE(detector_.hasObstacleOnPath(path, true)); + + // Condition: no obstacle on path + size_t path_length = 10; + path.points.reserve(path_length); + for (size_t i = 0; i < path_length; i++) { + tier4_planning_msgs::msg::PathPointWithLaneId path_point; + path_point.point.pose = createPose(static_cast(i), 0.0, 0.0, 0.0, 0.0, 0.0); + path.points.push_back(path_point); + } + EXPECT_FALSE(detector_.hasObstacleOnPath(path, false)); + + // Condition: obstacle on path + for (size_t i = 0; i < path_length; i++) { + path.points.at(i).point.pose.position.y = 8.0; + } + EXPECT_TRUE(detector_.hasObstacleOnPath(path, false)); +} From aaa8429d91670c813e6264a8382ae1ffab7179a9 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Thu, 10 Oct 2024 14:55:21 +0300 Subject: [PATCH 10/27] chore(obstacle_cruise_planner): add maintainer (#9077) --- planning/autoware_obstacle_cruise_planner/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 9999be9937111..90a5e666f5f44 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -8,6 +8,7 @@ Kosuke Takeuchi Satoshi Ota Yuki Takagi + Berkay Karaman Apache License 2.0 From d1e689225224c0b1b1ca48d313b4b299a28dbc86 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 11 Oct 2024 10:18:43 +0900 Subject: [PATCH 11/27] test(traffic_light): write test for utils (#9046) * refactor: separate utils file Signed-off-by: satoshi-ota * refactor: utils function output Signed-off-by: satoshi-ota * test: write test for utils Signed-off-by: satoshi-ota * chore: add doxygen Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../CMakeLists.txt | 10 + .../package.xml | 2 + .../src/scene.cpp | 148 ++------------ .../src/utils.cpp | 144 +++++++++++++ .../src/utils.hpp | 82 ++++++++ .../test/test_utils.cpp | 190 ++++++++++++++++++ 6 files changed, 441 insertions(+), 135 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt index db5b02916a103..6370dd5e6c21d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt @@ -9,8 +9,18 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp src/scene.cpp + src/utils.cpp ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package(INSTALL_TO_SHARE config) install(PROGRAMS diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 45a17e8e8f598..51992a6fa40d3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -37,8 +37,10 @@ traffic_light_utils visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index af16cd85741e5..af5eac943ec13 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -14,6 +14,8 @@ #include "scene.hpp" +#include "utils.hpp" + #include #include #include @@ -36,131 +38,6 @@ namespace autoware::behavior_velocity_planner { -namespace bg = boost::geometry; - -namespace -{ -bool getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) -{ - Eigen::Vector2d line_vec = line_point2 - line_point1; - Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); - output_point = base_point + backward_vec; - return true; -} - -std::optional findNearestCollisionPoint( - const LineString2d & line1, const LineString2d & line2, const Point2d & origin) -{ - std::vector collision_points; - bg::intersection(line1, line2, collision_points); - - if (collision_points.empty()) { - return std::nullopt; - } - - // check nearest collision point - Point2d nearest_collision_point; - double min_dist = 0.0; - - for (size_t i = 0; i < collision_points.size(); ++i) { - double dist = bg::distance(origin, collision_points.at(i)); - if (i == 0 || dist < min_dist) { - min_dist = dist; - nearest_collision_point = collision_points.at(i); - } - } - return nearest_collision_point; -} - -bool createTargetPoint( - const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, - const double offset, size_t & target_point_idx, Eigen::Vector2d & target_point) -{ - if (input.points.size() < 2) { - return false; - } - for (size_t i = 0; i < input.points.size() - 1; ++i) { - Point2d path_line_begin = { - input.points.at(i).point.pose.position.x, input.points.at(i).point.pose.position.y}; - Point2d path_line_end = { - input.points.at(i + 1).point.pose.position.x, input.points.at(i + 1).point.pose.position.y}; - LineString2d path_line = {path_line_begin, path_line_end}; - - // check nearest collision point - const auto nearest_collision_point = - findNearestCollisionPoint(path_line, stop_line, path_line_begin); - if (!nearest_collision_point) { - continue; - } - - // search target point index - target_point_idx = 0; - double length_sum = 0; - - Eigen::Vector2d point1, point2; - if (0 <= offset) { - point1 << nearest_collision_point->x(), nearest_collision_point->y(); - point2 << path_line_begin.x(), path_line_begin.y(); - length_sum += (point2 - point1).norm(); - for (size_t j = i; 0 < j; --j) { - if (offset < length_sum) { - target_point_idx = j + 1; - break; - } - point1 << input.points.at(j).point.pose.position.x, - input.points.at(j).point.pose.position.y; - point2 << input.points.at(j - 1).point.pose.position.x, - input.points.at(j - 1).point.pose.position.y; - length_sum += (point2 - point1).norm(); - } - } else { - point1 << nearest_collision_point->x(), nearest_collision_point->y(); - point2 << path_line_end.x(), path_line_end.y(); - length_sum -= (point2 - point1).norm(); - for (size_t j = i + 1; j < input.points.size() - 1; ++j) { - if (length_sum < offset) { - target_point_idx = j; - break; - } - point1 << input.points.at(j).point.pose.position.x, - input.points.at(j).point.pose.position.y; - point2 << input.points.at(j + 1).point.pose.position.x, - input.points.at(j + 1).point.pose.position.y; - length_sum -= (point2 - point1).norm(); - } - } - // create target point - getBackwardPointFromBasePoint( - point2, point1, point2, std::fabs(length_sum - offset), target_point); - return true; - } - return false; -} - -bool calcStopPointAndInsertIndex( - const tier4_planning_msgs::msg::PathWithLaneId & input_path, - const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, - const double & stop_line_extend_length, Eigen::Vector2d & stop_line_point, - size_t & stop_line_point_idx) -{ - LineString2d stop_line; - - for (size_t i = 0; i < lanelet_stop_lines.size() - 1; ++i) { - stop_line = planning_utils::extendLine( - lanelet_stop_lines[i], lanelet_stop_lines[i + 1], stop_line_extend_length); - - // Calculate stop pose and insert index, - // if there is a collision point between path and stop line - if (createTargetPoint(input_path, stop_line, offset, stop_line_point_idx, stop_line_point)) { - return true; - } - } - return false; -} -} // namespace - TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, @@ -193,12 +70,12 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); // Calculate stop pose and insert index - Eigen::Vector2d stop_line_point{}; - size_t stop_line_point_idx{}; - if (!calcStopPointAndInsertIndex( - input_path, lanelet_stop_lines, - planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m, - planner_data_->stop_line_extend_length, stop_line_point, stop_line_point_idx)) { + const auto stop_line = calcStopPointAndInsertIndex( + input_path, lanelet_stop_lines, + planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m, + planner_data_->stop_line_extend_length); + + if (!stop_line.has_value()) { RCLCPP_WARN_STREAM_ONCE( logger_, "Failed to calculate stop point and insert index for regulatory element id " << traffic_light_reg_elem_.id()); @@ -209,8 +86,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Calculate dist to stop pose geometry_msgs::msg::Point stop_line_point_msg; - stop_line_point_msg.x = stop_line_point.x(); - stop_line_point_msg.y = stop_line_point.y(); + stop_line_point_msg.x = stop_line.value().second.x(); + stop_line_point_msg.y = stop_line.value().second.y(); const double signed_arc_length_to_stop_point = autoware::motion_utils::calcSignedArcLength( input_path.points, self_pose->pose.position, stop_line_point_msg); setDistance(signed_arc_length_to_stop_point); @@ -226,7 +103,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } - first_ref_stop_path_point_index_ = stop_line_point_idx; + first_ref_stop_path_point_index_ = stop_line.value().first; // Check if stop is coming. const bool is_stop_signal = isStopSignal(); @@ -256,7 +133,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Decide whether to stop or pass even if a stop signal is received. if (!isPassthrough(signed_arc_length_to_stop_point)) { - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + *path = + insertStopPose(input_path, stop_line.value().first, stop_line.value().second, stop_reason); is_prev_state_stop_ = true; } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp new file mode 100644 index 0000000000000..c9eb93abb2e2f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.cpp @@ -0,0 +1,144 @@ +// 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 "utils.hpp" + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace bg = boost::geometry; + +auto getOffsetPoint(const Eigen::Vector2d & src, const Eigen::Vector2d & dst, const double length) + -> Eigen::Vector2d +{ + Eigen::Vector2d line_vec = dst - src; + Eigen::Vector2d backward_vec = length * line_vec.normalized(); + return src + backward_vec; +} + +auto findNearestCollisionPoint( + const LineString2d & line1, const LineString2d & line2, + const Point2d & origin) -> std::optional +{ + std::vector collision_points; + bg::intersection(line1, line2, collision_points); + + if (collision_points.empty()) { + return std::nullopt; + } + + // check nearest collision point + Point2d nearest_collision_point; + double min_dist = 0.0; + + for (size_t i = 0; i < collision_points.size(); ++i) { + double dist = bg::distance(origin, collision_points.at(i)); + if (i == 0 || dist < min_dist) { + min_dist = dist; + nearest_collision_point = collision_points.at(i); + } + } + return nearest_collision_point; +} + +auto createTargetPoint( + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const double offset) -> std::optional> +{ + if (input.points.size() < 2) { + return std::nullopt; + } + size_t target_point_idx{}; + for (size_t i = 0; i < input.points.size() - 1; ++i) { + Point2d path_line_begin = { + input.points.at(i).point.pose.position.x, input.points.at(i).point.pose.position.y}; + Point2d path_line_end = { + input.points.at(i + 1).point.pose.position.x, input.points.at(i + 1).point.pose.position.y}; + LineString2d path_line = {path_line_begin, path_line_end}; + + // check nearest collision point + const auto nearest_collision_point = + findNearestCollisionPoint(path_line, stop_line, path_line_begin); + if (!nearest_collision_point) { + continue; + } + + // search target point index + target_point_idx = 0; + double length_sum = 0; + + Eigen::Vector2d point1, point2; + if (0 <= offset) { + point1 << nearest_collision_point->x(), nearest_collision_point->y(); + point2 << path_line_begin.x(), path_line_begin.y(); + length_sum += (point2 - point1).norm(); + for (size_t j = i; 0 < j; --j) { + if (offset < length_sum) { + target_point_idx = j + 1; + break; + } + point1 << input.points.at(j).point.pose.position.x, + input.points.at(j).point.pose.position.y; + point2 << input.points.at(j - 1).point.pose.position.x, + input.points.at(j - 1).point.pose.position.y; + length_sum += (point2 - point1).norm(); + } + } else { + point1 << nearest_collision_point->x(), nearest_collision_point->y(); + point2 << path_line_end.x(), path_line_end.y(); + length_sum -= (point2 - point1).norm(); + for (size_t j = i + 1; j < input.points.size() - 1; ++j) { + if (length_sum < offset) { + target_point_idx = j; + break; + } + point1 << input.points.at(j).point.pose.position.x, + input.points.at(j).point.pose.position.y; + point2 << input.points.at(j + 1).point.pose.position.x, + input.points.at(j + 1).point.pose.position.y; + length_sum -= (point2 - point1).norm(); + } + } + // create target point + return std::make_pair( + target_point_idx, getOffsetPoint(point2, point1, std::fabs(length_sum - offset))); + } + return std::nullopt; +} + +auto calcStopPointAndInsertIndex( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, + const double & stop_line_extend_length) -> std::optional> +{ + LineString2d stop_line; + + for (size_t i = 0; i < lanelet_stop_lines.size() - 1; ++i) { + stop_line = planning_utils::extendLine( + lanelet_stop_lines[i], lanelet_stop_lines[i + 1], stop_line_extend_length); + + // Calculate stop pose and insert index, + // if there is a collision point between path and stop line + const auto output = createTargetPoint(input_path, stop_line, offset); + if (output.has_value()) { + return output; + } + } + return std::nullopt; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp new file mode 100644 index 0000000000000..c6655064d3ffa --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/utils.hpp @@ -0,0 +1,82 @@ +// 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +/** + * @brief create offset point. + * @param src point of vector. this point is a base point for offset point. + * @param dst point of vector. + * @param offset length. + * @return offset point. + */ +auto getOffsetPoint(const Eigen::Vector2d & src, const Eigen::Vector2d & dst, const double length) + -> Eigen::Vector2d; + +/** + * @brief find intersection point between a path segment and stop line and return the point. + * @param path segment. + * @param stop line. + * @param base point to find nearest intersection point. + * @return intersection point. if there is no intersection point, return std::nullopt. + */ +auto findNearestCollisionPoint( + const LineString2d & line1, const LineString2d & line2, + const Point2d & origin) -> std::optional; + +/** + * @brief find intersection point between path and stop line and return the point. + * @param input path. + * @param stop line. + * @param longitudinal offset. + * @return first: insert point index, second: insert point position. if there is no intersection + * point, return std::nullopt. + */ +auto createTargetPoint( + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const double offset) -> std::optional>; + +/** + * @brief find intersection point between path and stop line and return the point. + * @param input path. + * @param stop line. + * @param longitudinal offset. + * @param extend length to find intersection point between path and stop line. + * @return first: insert point index, second: insert point position. if there is no intersection + * point, return std::nullopt. + */ +auto calcStopPointAndInsertIndex( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, + const double & stop_line_extend_length) -> std::optional>; + +} // namespace autoware::behavior_velocity_planner + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp new file mode 100644 index 0000000000000..a5d09acc103e9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_utils.cpp @@ -0,0 +1,190 @@ +// Copyright 2022 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/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include + +namespace autoware::behavior_velocity_planner +{ + +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +using autoware::universe_utils::createPoint; +using autoware::universe_utils::createQuaternion; + +PathWithLaneId generatePath(const geometry_msgs::msg::Pose & pose) +{ + constexpr double interval_distance = 1.0; + + PathWithLaneId traj; + for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { + PathPointWithLaneId p; + p.point.pose = pose; + p.point.pose.position.x += s; + traj.points.push_back(p); + } + + return traj; +} + +TEST(BehaviorTrafficLightModuleUtilsTest, getOffsetPoint) +{ + constexpr double length = 2.0; + Point2d p1 = {0.0, 0.0}; + Point2d p2 = {1.0, 1.0}; + + const auto output = getOffsetPoint(p1, p2, length); + + EXPECT_DOUBLE_EQ(output.x(), 1.41421356237309505); + EXPECT_DOUBLE_EQ(output.y(), 1.41421356237309505); +} + +TEST(BehaviorTrafficLightModuleUtilsTest, findNearestCollisionPoint) +{ + { + Point2d p = {0.5, 0.5}; + LineString2d line1 = {{-1.0, 0.0}, {1.0, 0.0}}; + LineString2d line2 = {{0.0, -1.0}, {0.0, 1.0}}; + + const auto output = findNearestCollisionPoint(line1, line2, p); + + EXPECT_TRUE(output.has_value()); + EXPECT_DOUBLE_EQ(output.value().x(), 0.0); + EXPECT_DOUBLE_EQ(output.value().y(), 0.0); + } + + { + Point2d p = {0.5, 0.5}; + LineString2d line1 = {{-1.0, -1.0}, {-1.0, 1.0}}; + LineString2d line2 = {{1.0, -1.0}, {1.0, 1.0}}; + + const auto output = findNearestCollisionPoint(line1, line2, p); + + EXPECT_FALSE(output.has_value()); + } +} + +TEST(BehaviorTrafficLightModuleUtilsTest, createTargetPoint) +{ + const auto pose = geometry_msgs::build() + .position(createPoint(0.0, 0.0, 0.0)) + .orientation(createQuaternion(0.0, 0.0, 0.0, 1.0)); + const auto path = generatePath(pose); + + { + constexpr double offset = 1.75; + LineString2d line = {{5.5, -1.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(PathWithLaneId{}, line, offset); + + EXPECT_FALSE(output.has_value()); + } + + { + constexpr double offset = 1.75; + LineString2d line = {{5.5, -1.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(path, line, offset); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(4)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 3.75); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + constexpr double offset = -1.75; + LineString2d line = {{5.5, -1.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(path, line, offset); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(8)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 7.25); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + constexpr double offset = 1.75; + LineString2d line = {{5.5, 2.0}, {5.5, 1.0}}; + const auto output = createTargetPoint(path, line, offset); + + EXPECT_FALSE(output.has_value()); + } +} + +TEST(BehaviorTrafficLightModuleUtilsTest, calcStopPointAndInsertIndex) +{ + const auto pose = geometry_msgs::build() + .position(createPoint(0.0, 0.0, 0.0)) + .orientation(createQuaternion(0.0, 0.0, 0.0, 1.0)); + const auto path = generatePath(pose); + constexpr double offset = 1.75; + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, -1.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 0.0; + const auto output = calcStopPointAndInsertIndex(PathWithLaneId{}, line, offset, extend_length); + + EXPECT_FALSE(output.has_value()); + } + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, -1.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 0.0; + const auto output = calcStopPointAndInsertIndex(path, line, offset, extend_length); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(4)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 3.75); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, 2.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 2.0; + const auto output = calcStopPointAndInsertIndex(path, line, offset, extend_length); + + EXPECT_TRUE(output.has_value()); + EXPECT_EQ(output.value().first, size_t(4)); + EXPECT_DOUBLE_EQ(output.value().second.x(), 3.75); + EXPECT_DOUBLE_EQ(output.value().second.y(), 0.0); + } + + { + lanelet::Points3d basic_line; + basic_line.emplace_back(lanelet::InvalId, 5.5, 2.0, 0.0); + basic_line.emplace_back(lanelet::InvalId, 5.5, 1.0, 0.0); + + const auto line = lanelet::LineString3d(lanelet::InvalId, basic_line); + constexpr double extend_length = 0.0; + const auto output = calcStopPointAndInsertIndex(path, line, offset, extend_length); + + EXPECT_FALSE(output.has_value()); + } +} + +} // namespace autoware::behavior_velocity_planner From d01ae0d5e4cf990f752f75d9c16dfadb53e22a58 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 11 Oct 2024 13:15:16 +0900 Subject: [PATCH 12/27] fix(crosswalk): fix passing direction calclation for the objects (#9071) Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.cpp | 33 ++++++++----------- 1 file changed, 13 insertions(+), 20 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index f56552485b5fc..4ec3e43ee91ca 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -353,13 +353,16 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( if (collision_state != CollisionState::YIELD) { continue; } + if ( isVehicleType(object.classification) && ego_crosswalk_passage_direction && collision_point.crosswalk_passage_direction) { - const double direction_diff = autoware::universe_utils::normalizeRadian( + double direction_diff = std::abs(std::fmod( collision_point.crosswalk_passage_direction.value() - - ego_crosswalk_passage_direction.value()); - if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { + ego_crosswalk_passage_direction.value(), + M_PI_2)); + direction_diff = std::min(direction_diff, M_PI_2 - direction_diff); + if (direction_diff < planner_param_.vehicle_object_cross_angle_threshold) { continue; } } @@ -670,8 +673,8 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( const PathWithLaneId & path) const { - auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) - -> std::optional> { + auto findIntersectPoint = + [&](const lanelet::ConstLineString3d line) -> std::optional { const auto line_start = autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); const auto line_end = @@ -682,29 +685,19 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( if (const auto intersect = autoware::universe_utils::intersect(line_start, line_end, start, end); intersect.has_value()) { - return std::make_optional(std::make_pair(i, intersect.value())); + return intersect; } } return std::nullopt; }; - const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); - const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + const auto pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto pt2 = findIntersectPoint(crosswalk_.rightBound()); - if (!intersect_pt1 || !intersect_pt2) { + if (!pt1 || !pt2) { return std::nullopt; } - const auto idx1 = intersect_pt1.value().first; - const auto idx2 = intersect_pt2.value().first; - - const auto min_idx = std::min(idx1, idx2); - const auto dist1 = calcSignedArcLength(path.points, min_idx, intersect_pt1.value().second); - const auto dist2 = calcSignedArcLength(path.points, min_idx, intersect_pt2.value().second); - - const auto & front = dist1 > dist2 ? intersect_pt2.value().second : intersect_pt1.value().second; - const auto & back = dist1 > dist2 ? intersect_pt1.value().second : intersect_pt2.value().second; - - return std::atan2(back.y - front.y, back.x - front.x); + return std::atan2(pt2->y - pt1->y, pt2->x - pt1->x); } std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( From dbcf7aa9da79468ff28ee89483af5a2264384e0e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 11 Oct 2024 14:18:48 +0900 Subject: [PATCH 13/27] fix(lane_change): insert stop for current lanes object (RT0-33761) (#9070) * RT0-33761 fix lc insert stop for current lanes object Signed-off-by: Zulfaqar Azmi * fix wrong value used for comparison Signed-off-by: Zulfaqar Azmi * ignore current lane object that is not on ego's path Signed-off-by: Zulfaqar Azmi * remove print Signed-off-by: Zulfaqar Azmi * update readme Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * revert is_within_vel_th removal Signed-off-by: Zulfaqar Azmi * fix flowchart too wide Signed-off-by: Zulfaqar Azmi * rename variable in has_blocking_target_object_for_stopping Signed-off-by: Zulfaqar Azmi * Add docstring and rename function Signed-off-by: Zulfaqar Azmi * change color Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../README.md | 91 +++++++- .../scene.hpp | 6 +- .../utils/base_class.hpp | 2 +- .../utils/utils.hpp | 42 ++++ .../src/interface.cpp | 2 +- .../src/scene.cpp | 205 ++++++++---------- .../src/utils/utils.cpp | 65 ++++++ 7 files changed, 286 insertions(+), 127 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 02280f7ffa697..a52d842b5dcb9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -195,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if ego is stuck in the current lanes then (yes) +if (ego is stuck in the current lanes) then (yes) :Return **sampled acceleration values**; stop else (no) @@ -540,14 +540,65 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) -#### Stopping position when an object exists ahead +### Stopping behavior -When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. -The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. +The stopping behavior of the ego vehicle is determined based on various factors, such as the number of lane changes required, the presence of obstacles, and the position of blocking objects in relation to the lane change plan. The objective is to choose a suitable stopping point that allows for a safe and effective lane change while adapting to different traffic scenarios. -##### When the ego vehicle is near the end of the lane change +The following flowchart and subsections explain the conditions for deciding where to insert a stop point when an obstacle is ahead. -Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Inserting Stop Point + +start +if (number of lane changes is zero?) then (YES) +stop +else (NO) +endif + +if (do we want to insert stop point in current lanes?) then (NO) +#LightPink:Insert stop point at next lane change terminal start.; +stop +else (YES) +endif + +if (Is there leading object in the current lanes that blocks ego's path?) then (NO) +#LightPink:Insert stop point at terminal stop.; +stop +else (YES) +endif + +if (Blocking object's position is after target lane's start position?) then (NO) +#LightPink:Insert stop at the target lane's start position.; +stop +else (YES) +endif + +if (Blocking object's position is before terminal stop position?) then (NO) +#LightPink:Insert stop at the terminal stop position; +stop +else (YES) +endif + +if (Are there target lane objects between the ego and the blocking object?) then (NO) +#LightGreen:Insert stop behind the blocking object; +stop +else (YES) +#LightPink:Insert stop at terminal stop; +stop +@enduml +``` + +#### Ego vehicle's stopping position when an object exists ahead + +When the ego vehicle encounters an obstacle ahead, it stops while maintaining a safe distance to prepare for a possible lane change. The exact stopping position depends on factors like whether the target lane is clear or if the lane change needs to be delayed. The following explains how different stopping scenarios are handled: + +##### When the near the end of the lane change + +Whether the target lane has obstacles or is clear, the ego vehicle stops while keeping a safe distance from the obstacle ahead, ensuring there is enough room for the lane change. ![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) @@ -555,20 +606,40 @@ Regardless of the presence or absence of objects in the lane change target lane, ##### When the ego vehicle is not near the end of the lane change -If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. +The ego vehicle stops while maintaining a safe distance from the obstacle ahead, ensuring there is enough space for a lane change. + +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +#### Ego vehicle's stopping position when an object exists in the lane changing section + +If there are objects within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without maintaining the usual distance for a lane change. + +##### When near the end of the lane change + +Regardless of whether there are obstacles in the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead, allowing for the lane change. + +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) + +##### When not near the end of the lane change + +If there are no obstacles in the lane change section of the target lane, the ego vehicle stops while keeping a safe distance from the obstacle ahead to accommodate the lane change. ![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) -If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. +If there are obstacles within the lane change section of the target lane, the ego vehicle stops closer to the obstacle ahead, without keeping the usual distance needed for a lane change. ![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) -##### When the target lane is far away +#### When the target lane is far away -When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. +If the target lane for the lane change is far away and not next to the current lane, the ego vehicle stops closer to the obstacle ahead, as maintaining the usual distance for a lane change is not necessary. ![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) + ### Lane Change When Stuck The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index cc06d0b114d7b..3b414235a9f54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -70,7 +70,9 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + + void insert_stop_point_on_current_lanes(PathWithLaneId & path); PathWithLaneId getReferencePath() const override; @@ -209,7 +211,7 @@ class NormalLaneChange : public LaneChangeBase std::pair calcCurrentMinMaxAcceleration() const; - void setStopPose(const Pose & stop_pose); + void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); void updateStopTime(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index c124353e2873a..aaf9df7144d74 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -127,7 +127,7 @@ class LaneChangeBase virtual void updateSpecialData() {} - virtual void insertStopPoint( + virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, [[maybe_unused]] PathWithLaneId & path) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 3219dc7e777f1..729eaa5ca8268 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -266,6 +266,48 @@ ExtendedPredictedObjects transform_to_extended_objects( double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); + +/** + * @brief Calculates the minimum distance to a stationary object in the current lanes. + * + * This function determines the closest distance from the ego vehicle to a stationary object + * in the current lanes. It checks if the object is within the stopping criteria based on its + * velocity and calculates the distance while accounting for the object's size. Only objects + * positioned after the specified distance to the target lane's start are considered. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the current + * lane. + * @param dist_to_target_lane_start The distance to the start of the target lane from the ego + * vehicle. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return The minimum distance to a stationary object in the current lanes. If no valid object is + * found, returns the maximum possible double value. + */ +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path); + +/** + * @brief Checks if there is an object in the target lane that influences the decision to insert a + * stop point. + * + * This function determines whether any objects exist in the target lane that would affect + * the decision to place a stop point behind a blocking object in the current lane. + * + * @param common_data_ptr Pointer to the common data structure containing parameters for the lane + * change. + * @param filtered_objects A collection of objects filtered by lanes, including those in the target + * lane. + * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. + * @param path The current path of the ego vehicle, containing path points and lane information. + * @return true if there is an object in the target lane that influences the stop point decision; + * otherwise, false. + */ +bool has_blocking_target_object( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double stop_arc_length, const PathWithLaneId & path); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 09550a135ba3c..8fb1459f62c98 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -154,7 +154,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 5a9ee485320b8..532a99a67195b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -370,14 +370,14 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); - insertStopPoint(get_current_lanes(), prev_module_output_.path); + insert_stop_point(get_current_lanes(), prev_module_output_.path); return prev_module_output_; } auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(get_current_lanes(), output.path); + insert_stop_point(get_current_lanes(), output.path); } else { output.path = status_.lane_change_path.path; @@ -395,10 +395,9 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); - setStopPose(stop_point.point.pose); + set_stop_pose(stop_dist + current_dist, output.path); } else { - insertStopPoint(get_target_lanes(), output.path); + insert_stop_point(get_target_lanes(), output.path); } } @@ -432,7 +431,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c current_drivable_area_info, prev_module_output_.drivable_area_info); } -void NormalLaneChange::insertStopPoint( +void NormalLaneChange::insert_stop_point( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -446,131 +445,109 @@ void NormalLaneChange::insertStopPoint( return; } - const auto [_, lanes_dist_buffer] = - calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, lanelets); + const auto & current_lanes = get_current_lanes(); + const auto is_current_lane = lanelets.front().id() == current_lanes.front().id() && + lanelets.back().id() == current_lanes.back().id(); + + // if input is not current lane, we can just insert the points at terminal. + if (!is_current_lane) { + const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - + common_data_ptr_->transient_data.next_dist_buffer.min; + set_stop_pose(arc_length_to_stop_pose, path); + return; + } + + insert_stop_point_on_current_lanes(path); +} - const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { - return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); +void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +{ + const auto & path_front_pose = path.points.front().point.pose; + const auto & center_line = common_data_ptr_->current_lanes_path.points; + const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target.position); }; - // If lanelets.back() is in goal route section, get distance to goal. - // Otherwise, get distance to end of lane. - double distance_to_terminal = 0.0; - if (route_handler->isInGoalRouteSection(lanelets.back())) { - const auto goal = route_handler->getGoalPose(); - distance_to_terminal = getDistanceAlongLanelet(goal); - } else { - distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); - } + const auto & transient_data = common_data_ptr_->transient_data; + const auto & lanes_ptr = common_data_ptr_->lanes_ptr; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(); - double stopping_distance = distance_to_terminal - lanes_dist_buffer.min - stop_point_buffer; + const auto dist_to_terminal = std::invoke([&]() -> double { + const auto target_pose = (lanes_ptr->current_lane_in_goal_section) + ? common_data_ptr_->route_handler_ptr->getGoalPose() + : center_line.back().point.pose; + return get_arc_length_along_lanelet(target_pose); + }); - const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { - const double distance_to_last_fit_width = - utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanelets, path.points.front().point.pose, planner_data_->parameters); - stopping_distance = std::min(stopping_distance, distance_to_last_fit_width); - } + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto min_dist_buffer = transient_data.current_dist_buffer.min; + const auto dist_to_terminal_start = + dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); - const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; + const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return utils::lane_change::calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + } + return std::numeric_limits::max(); + }); - if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + if (filtered_objects_.current_lane.empty()) { + set_stop_pose(dist_to_terminal_stop, path); return; } - // calculate minimum distance from path front to the stationary object on the ego lane. - const auto distance_to_ego_lane_obj = [&]() -> double { - double distance_to_obj = distance_to_terminal; - const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); - - for (const auto & object : target_objects.current_lane) { - // check if stationary - const auto obj_v = std::abs(object.initial_twist.linear.x); - if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { - continue; - } + const auto dist_to_target_lane_start = std::invoke([&]() -> double { + const auto & front_lane = lanes_ptr->target_neighbor.front(); + const auto target_front = + utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); + return get_arc_length_along_lanelet(target_front); + }); - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); + const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( + common_data_ptr_, filtered_objects_, dist_to_target_lane_start, path); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } + // margin with leading vehicle + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params_for_parked); - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + const auto stop_margin = transient_data.lane_changing_length.min + + lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist + + bpp_param_ptr->base_link2front; + const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; - // ignore backward object - if (current_distance_to_obj < distance_to_ego) { - continue; - } - distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); - } - } - return distance_to_obj; - }(); - - // Need to stop before blocking obstacle - if (distance_to_ego_lane_obj < distance_to_terminal) { - // consider rss distance when the LC need to avoid obstacles - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); - - const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lanes_dist_buffer.min - - lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - - getCommonParam().base_link2front; - - // If the target lane in the lane change section is blocked by a stationary obstacle, there - // is no reason for stopping with a lane change margin. Instead, stop right behind the - // obstacle. - // ---------------------------------------------------------- - // [obj]> - // ---------------------------------------------------------- - // [ego]> | <--- lane change margin ---> [obj]> - // ---------------------------------------------------------- - const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), - [&](const auto & o) { - const auto v = std::abs(o.initial_twist.linear.x); - if (v > lane_change_parameters_->stopped_object_velocity_threshold) { - return false; - } + if (stop_arc_length_behind_obj < dist_to_target_lane_start) { + set_stop_pose(dist_to_target_lane_start, path); + return; + } - // target_objects includes objects out of target lanes, so filter them out - if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(get_target_lanes()) - .polygon2d() - .basicPolygon())) { - return false; - } + if (stop_arc_length_behind_obj > dist_to_terminal_stop) { + set_stop_pose(dist_to_terminal_stop, path); + return; + } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); - return stopping_distance_for_obj < distance_to_target_lane_obj && - distance_to_target_lane_obj < distance_to_ego_lane_obj; - }); + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- stop margin ---> [obj]> + // ---------------------------------------------------------- + const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( + common_data_ptr_, filtered_objects_, stop_arc_length_behind_obj, path); - if (!has_blocking_target_lane_obj) { - stopping_distance = stopping_distance_for_obj; - } + if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { + set_stop_pose(dist_to_terminal_stop, path); + return; } - if (stopping_distance > 0.0) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); - } + set_stop_pose(stop_arc_length_behind_obj, path); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -1109,6 +1086,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const }); } + // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior utils::path_safety_checker::filterObjects( filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); @@ -2182,9 +2160,10 @@ bool NormalLaneChange::is_valid_start_point( boost::geometry::covered_by(lc_start_point, target_lane_poly); } -void NormalLaneChange::setStopPose(const Pose & stop_pose) +void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) { - lane_change_stop_pose_ = stop_pose; + const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); + lane_change_stop_pose_ = stop_point.point.pose; } void NormalLaneChange::updateStopTime() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 20bc982445b07..c4fe5fa9902c5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -1250,4 +1250,69 @@ double get_distance_to_next_regulatory_element( return distance; } + +double get_min_dist_to_current_lanes_obj( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double dist_to_target_lane_start, const PathWithLaneId & path) +{ + const auto & path_points = path.points; + auto min_dist_to_obj = std::numeric_limits::max(); + for (const auto & object : filtered_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.linear.x); + if (obj_v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + continue; + } + + // provide "estimation" based on size of object + const auto dist_to_obj = + motion_utils::calcSignedArcLength( + path_points, path_points.front().point.pose.position, object.initial_pose.position) - + (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + for (const auto & polygon_p : object.initial_polygon.outer()) { + const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); + + // ignore if the point is not on ego path + if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; + } + + const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); + min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + } + } + return min_dist_to_obj; +} + +bool has_blocking_target_object( + const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const double stop_arc_length, const PathWithLaneId & path) +{ + return std::any_of( + filtered_objects.target_lane_leading.begin(), filtered_objects.target_lane_leading.end(), + [&](const auto & object) { + const auto v = std::abs(object.initial_twist.linear.x); + if (v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { + return false; + } + + // filtered_objects includes objects out of target lanes, so filter them out + if (boost::geometry::disjoint( + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target.value())) { + return false; + } + + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); +} } // namespace autoware::behavior_path_planner::utils::lane_change From f5818f71e4b5212815b012895cec409de29a2c78 Mon Sep 17 00:00:00 2001 From: Kazunori-Nakajima <169115284+Kazunori-Nakajima@users.noreply.github.com> Date: Fri, 11 Oct 2024 14:56:11 +0900 Subject: [PATCH 14/27] fix(.github): change `runs-on` value from `ubuntu-latest` to `ubuntu-22.04` (#9080) fix(.github): change value from to Signed-off-by: Kasunori-Nakajima --- .github/workflows/build-and-test-differential.yaml | 2 +- .github/workflows/cancel-previous-workflows.yaml | 2 +- .github/workflows/check-build-depends.yaml | 2 +- .github/workflows/clang-tidy-pr-comments-manually.yaml | 2 +- .github/workflows/clang-tidy-pr-comments.yaml | 2 +- .github/workflows/comment-on-pr.yaml | 2 +- .github/workflows/cppcheck-differential.yaml | 2 +- .github/workflows/cppcheck-weekly.yaml | 2 +- .github/workflows/dco.yaml | 2 +- .github/workflows/delete-closed-pr-docs.yaml | 2 +- .github/workflows/deploy-docs.yaml | 2 +- .github/workflows/github-release.yaml | 2 +- .github/workflows/json-schema-check.yaml | 6 +++--- .github/workflows/pr-agent.yaml | 2 +- .github/workflows/pr-labeler.yaml | 2 +- .github/workflows/pre-commit-autoupdate.yaml | 2 +- .github/workflows/pre-commit-optional.yaml | 2 +- .github/workflows/pre-commit.yaml | 2 +- .github/workflows/spell-check-daily.yaml | 2 +- .github/workflows/spell-check-differential.yaml | 2 +- .github/workflows/sync-files.yaml | 2 +- .github/workflows/update-codeowners-from-packages.yaml | 2 +- 22 files changed, 24 insertions(+), 24 deletions(-) diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 5e1f3e2c093a5..cda1666603926 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -75,7 +75,7 @@ jobs: clang-tidy-differential: needs: build-and-test-differential - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe-cuda steps: - name: Set PR fetch depth diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index 44983f7deadcb..bd2463d5a8eea 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -5,7 +5,7 @@ on: jobs: cancel-previous-workflows: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.12.1 diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 81618a1db0eea..74c97e3bf4be4 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -7,7 +7,7 @@ on: jobs: check-build-depends: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 container: ${{ matrix.container }} strategy: fail-fast: false diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index 9bccd972becde..a4df9f9dec3ed 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -8,7 +8,7 @@ on: required: true jobs: clang-tidy-pr-comments-manually: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index baaa0fb8e7744..bf2ed81d7ae48 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -10,7 +10,7 @@ on: jobs: clang-tidy-pr-comments: if: ${{ github.event.workflow_run.event == 'pull_request' && contains(fromJson('["success", "failure"]'), github.event.workflow_run.conclusion) }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/comment-on-pr.yaml b/.github/workflows/comment-on-pr.yaml index e517016a300d9..c80141ddac6b0 100644 --- a/.github/workflows/comment-on-pr.yaml +++ b/.github/workflows/comment-on-pr.yaml @@ -4,7 +4,7 @@ on: jobs: comment-on-pr: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 permissions: pull-requests: write steps: diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 8c2edd3c76724..09b9b6e983f06 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -5,7 +5,7 @@ on: jobs: cppcheck-differential: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Set PR fetch depth diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml index 573dcc54aa8c0..28be3897ae4f8 100644 --- a/.github/workflows/cppcheck-weekly.yaml +++ b/.github/workflows/cppcheck-weekly.yaml @@ -7,7 +7,7 @@ on: jobs: cppcheck-weekly: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Checkout code diff --git a/.github/workflows/dco.yaml b/.github/workflows/dco.yaml index db7ace467c658..e95b394ac63d0 100644 --- a/.github/workflows/dco.yaml +++ b/.github/workflows/dco.yaml @@ -4,7 +4,7 @@ on: pull_request: jobs: dco: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index b7b009fb00263..192e138a83c22 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -7,7 +7,7 @@ on: jobs: delete-closed-pr-docs: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index b48d70dbacb0c..771b4bd36ca9d 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -27,7 +27,7 @@ jobs: deploy-docs: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index b426d0cba6614..4b1d7f47c6c0c 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -15,7 +15,7 @@ on: jobs: github-release: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Set tag name id: set-tag-name diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml index 5c223edf1eec2..be9b8e3ce20fe 100644 --- a/.github/workflows/json-schema-check.yaml +++ b/.github/workflows/json-schema-check.yaml @@ -6,7 +6,7 @@ on: jobs: check-if-relevant-files-changed: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 outputs: run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} steps: @@ -22,7 +22,7 @@ jobs: json-schema-check: needs: check-if-relevant-files-changed if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 @@ -33,7 +33,7 @@ jobs: no-relevant-changes: needs: check-if-relevant-files-changed if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Dummy step run: echo "No relevant changes, passing check" diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index 98646d8161da2..5cd0845b3d9b4 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -13,7 +13,7 @@ jobs: pr_agent_job: needs: prevent-no-label-execution-pr-agent if: ${{ needs.prevent-no-label-execution-pr-agent.outputs.run == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 permissions: issues: write pull-requests: write diff --git a/.github/workflows/pr-labeler.yaml b/.github/workflows/pr-labeler.yaml index d45067bee59ed..cc2b26fd3e422 100644 --- a/.github/workflows/pr-labeler.yaml +++ b/.github/workflows/pr-labeler.yaml @@ -8,7 +8,7 @@ on: jobs: label: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - uses: actions/labeler@v4 with: diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml index 23b403f2a52af..8d57a53b5ccab 100644 --- a/.github/workflows/pre-commit-autoupdate.yaml +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -14,7 +14,7 @@ jobs: pre-commit-autoupdate: needs: check-secret if: ${{ needs.check-secret.outputs.set == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 38738196a0bd3..12f536c551646 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -5,7 +5,7 @@ on: jobs: pre-commit-optional: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index c724885fcb3e4..4d005e849b5ec 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -6,7 +6,7 @@ on: jobs: pre-commit: if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/.github/workflows/spell-check-daily.yaml b/.github/workflows/spell-check-daily.yaml index dcee449abc728..8c0373594a90e 100644 --- a/.github/workflows/spell-check-daily.yaml +++ b/.github/workflows/spell-check-daily.yaml @@ -7,7 +7,7 @@ on: jobs: spell-check-daily: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index ee9d451ba9b9e..3542659332532 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -5,7 +5,7 @@ on: jobs: spell-check-differential: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 51e523b8031bf..0cffbcd2a269f 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -14,7 +14,7 @@ jobs: sync-files: needs: check-secret if: ${{ needs.check-secret.outputs.set == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 8b3d2407fbc75..760a647ffbf56 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -14,7 +14,7 @@ jobs: update-codeowners-from-packages: needs: check-secret if: ${{ needs.check-secret.outputs.set == 'true' }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - name: Generate token id: generate-token From 543b45eea074ffd511863c6cac18916f8e38ccef Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 11 Oct 2024 16:37:01 +0900 Subject: [PATCH 15/27] refactor(autoware_autonomous_emergency_braking): rename info_marker_publisher to virtual_wall_publisher (#9078) Signed-off-by: kyoichi-sugahara --- .../autoware/autonomous_emergency_braking/node.hpp | 2 +- .../autoware_autonomous_emergency_braking/src/node.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 214a6dc309210..32bedd15c5c53 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -341,7 +341,7 @@ class AEB : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; - rclcpp::Publisher::SharedPtr info_marker_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; rclcpp::Publisher::SharedPtr debug_processing_time_detail_pub_; rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index b3988ba238fe7..df78ffc156802 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -138,7 +138,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) pub_obstacle_pointcloud_ = this->create_publisher("~/debug/obstacle_pointcloud", 1); debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); - info_marker_publisher_ = this->create_publisher("~/info/markers", 1); + virtual_wall_publisher_ = this->create_publisher("~/virtual_wall", 1); debug_rss_distance_publisher_ = this->create_publisher("~/debug/rss_distance", 1); } @@ -398,7 +398,7 @@ bool AEB::fetchLatestData() void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; - MarkerArray info_markers; + MarkerArray virtual_wall_marker; checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -414,7 +414,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) addCollisionMarker(data.value(), debug_markers); } } - addVirtualStopWallMarker(info_markers); + addVirtualStopWallMarker(virtual_wall_marker); } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -423,7 +423,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) // publish debug markers debug_marker_publisher_->publish(debug_markers); - info_marker_publisher_->publish(info_markers); + virtual_wall_publisher_->publish(virtual_wall_marker); } bool AEB::checkCollision(MarkerArray & debug_markers) From a71ef19650009d3862c4c2dd53aa5c0f95fa4ae9 Mon Sep 17 00:00:00 2001 From: melike tanrikulu <41450930+meliketanrikulu@users.noreply.github.com> Date: Fri, 11 Oct 2024 13:32:55 +0300 Subject: [PATCH 16/27] docs(autoware_pose_covariance_modifier): fix gt symbol (#9082) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Melike Tanrıkulu --- localization/autoware_pose_covariance_modifier/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index 60ecf4bde3508..201fb8bb37103 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -80,12 +80,12 @@ graph TD pc1{{"gnss_pose_yaw
stddev"}} pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z
stddev"}} pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy
stddev"}} - pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") + pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose") pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose (_with modified covariance_)`") - pc3 -->|"> 0.2 m"| ndt_pose - pc1 -->|"> 0.3 rad"| ndt_pose + pc3 -->|"> 0.2 m"| ndt_pose + pc1 -->|"> 0.3 rad"| ndt_pose end pose_covariance_modifier_node -->|"/localization/pose_estimator/pose_with_covariance"| ekf_localizer["ekf_localizer"] From d137f7c732ddad85b2b002b5f4395188a4686bed Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 15 Oct 2024 03:27:56 +0200 Subject: [PATCH 17/27] refactor(osqp_interface): added autoware prefix to osqp_interface (#8958) Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 14 +++++------ .../design/osqp_interface-design.md | 2 +- .../osqp_interface/csc_matrix_conv.hpp | 18 +++++--------- .../osqp_interface/osqp_interface.hpp | 20 ++++++---------- .../osqp_interface/visibility_control.hpp | 6 ++--- .../package.xml | 2 +- .../src/csc_matrix_conv.cpp | 12 +++------- .../src/osqp_interface.cpp | 14 ++++------- .../test/test_csc_matrix_conv.cpp | 18 +++++++------- .../test/test_osqp_interface.cpp | 24 +++++++++---------- .../qp_solver/qp_solver_osqp.hpp | 4 ++-- .../package.xml | 2 +- .../package.xml | 2 +- .../velocity_optimizer.hpp | 4 ++-- .../package.xml | 2 +- .../autoware/path_optimizer/mpt_optimizer.hpp | 4 ++-- planning/autoware_path_optimizer/package.xml | 2 +- .../src/mpt_optimizer.cpp | 14 +++++------ .../autoware/path_smoother/elastic_band.hpp | 4 ++-- planning/autoware_path_smoother/package.xml | 2 +- .../src/elastic_band.cpp | 2 +- .../package.xml | 2 +- .../autoware/velocity_smoother/node.hpp | 2 +- .../smoother/l2_pseudo_jerk_smoother.hpp | 4 ++-- .../smoother/linf_pseudo_jerk_smoother.hpp | 4 ++-- .../autoware_velocity_smoother/package.xml | 2 +- 26 files changed, 81 insertions(+), 105 deletions(-) rename common/{osqp_interface => autoware_osqp_interface}/CMakeLists.txt (78%) rename common/{osqp_interface => autoware_osqp_interface}/design/osqp_interface-design.md (96%) rename common/{osqp_interface/include => autoware_osqp_interface/include/autoware}/osqp_interface/csc_matrix_conv.hpp (83%) rename common/{osqp_interface/include => autoware_osqp_interface/include/autoware}/osqp_interface/osqp_interface.hpp (96%) rename common/{osqp_interface/include => autoware_osqp_interface/include/autoware}/osqp_interface/visibility_control.hpp (89%) rename common/{osqp_interface => autoware_osqp_interface}/package.xml (96%) rename common/{osqp_interface => autoware_osqp_interface}/src/csc_matrix_conv.cpp (94%) rename common/{osqp_interface => autoware_osqp_interface}/src/osqp_interface.cpp (98%) rename common/{osqp_interface => autoware_osqp_interface}/test/test_csc_matrix_conv.cpp (93%) rename common/{osqp_interface => autoware_osqp_interface}/test/test_osqp_interface.cpp (85%) diff --git a/common/osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt similarity index 78% rename from common/osqp_interface/CMakeLists.txt rename to common/autoware_osqp_interface/CMakeLists.txt index 5a4231c15e8fd..e9ed0ce25f2f8 100644 --- a/common/osqp_interface/CMakeLists.txt +++ b/common/autoware_osqp_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(osqp_interface) +project(autoware_osqp_interface) find_package(autoware_cmake REQUIRED) autoware_package() @@ -17,9 +17,9 @@ set(OSQP_INTERFACE_LIB_SRC ) set(OSQP_INTERFACE_LIB_HEADERS - include/osqp_interface/csc_matrix_conv.hpp - include/osqp_interface/osqp_interface.hpp - include/osqp_interface/visibility_control.hpp + include/autoware/osqp_interface/csc_matrix_conv.hpp + include/autoware/osqp_interface/osqp_interface.hpp + include/autoware/osqp_interface/visibility_control.hpp ) ament_auto_add_library(${PROJECT_NAME} SHARED @@ -28,18 +28,18 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) -target_include_directories(osqp_interface +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${OSQP_INCLUDE_DIR}" "${EIGEN3_INCLUDE_DIR}" ) -ament_target_dependencies(osqp_interface +ament_target_dependencies(${PROJECT_NAME} Eigen3 osqp_vendor ) -# crucial so downstream package builds because osqp_interface exposes osqp.hpp +# crucial so downstream package builds because autoware_osqp_interface exposes osqp.hpp ament_export_include_directories("${OSQP_INCLUDE_DIR}") # crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a ament_export_libraries(osqp::osqp) diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md similarity index 96% rename from common/osqp_interface/design/osqp_interface-design.md rename to common/autoware_osqp_interface/design/osqp_interface-design.md index e9ff4a2a3bc3a..887a4b4d9af3f 100644 --- a/common/osqp_interface/design/osqp_interface-design.md +++ b/common/autoware_osqp_interface/design/osqp_interface-design.md @@ -1,6 +1,6 @@ # Interface for the OSQP library -This is the design document for the `osqp_interface` package. +This is the design document for the `autoware_osqp_interface` package. ## Purpose / Use cases diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp similarity index 83% rename from common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp index a919bc1f1c038..8c21553152d70 100644 --- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp @@ -12,21 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ -#define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#ifndef AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#define AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#include "autoware/osqp_interface/visibility_control.hpp" #include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') -#include "osqp_interface/visibility_control.hpp" #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { /// \brief Compressed-Column-Sparse Matrix struct OSQP_INTERFACE_PUBLIC CSC_Matrix @@ -46,8 +42,6 @@ OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & /// \brief Print the given CSC matrix to the standard output OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface -#endif // OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp similarity index 96% rename from common/osqp_interface/include/osqp_interface/osqp_interface.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp index f126ba9329d3e..ff3013bd61514 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#ifndef AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#define AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#include "autoware/osqp_interface/csc_matrix_conv.hpp" +#include "autoware/osqp_interface/visibility_control.hpp" #include "osqp/osqp.h" -#include "osqp_interface/csc_matrix_conv.hpp" -#include "osqp_interface/visibility_control.hpp" #include #include @@ -28,11 +28,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { constexpr c_float INF = 1e30; @@ -193,8 +189,6 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface void logUnsolvedStatus(const std::string & prefix_message = "") const; }; -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface -#endif // OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp similarity index 89% rename from common/osqp_interface/include/osqp_interface/visibility_control.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp index 070a5c5f1542c..a6cdaa8a0a74e 100644 --- a/common/osqp_interface/include/osqp_interface/visibility_control.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/osqp_interface/package.xml b/common/autoware_osqp_interface/package.xml similarity index 96% rename from common/osqp_interface/package.xml rename to common/autoware_osqp_interface/package.xml index 41ee5bb9055a6..d49ce63bd8c93 100644 --- a/common/osqp_interface/package.xml +++ b/common/autoware_osqp_interface/package.xml @@ -1,7 +1,7 @@ - osqp_interface + autoware_osqp_interface 1.0.0 Interface for the OSQP solver Maxime CLEMENT diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp similarity index 94% rename from common/osqp_interface/src/csc_matrix_conv.cpp rename to common/autoware_osqp_interface/src/csc_matrix_conv.cpp index 1944face4516b..c6e1a0a42d938 100644 --- a/common/osqp_interface/src/csc_matrix_conv.cpp +++ b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "osqp_interface/csc_matrix_conv.hpp" +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include #include @@ -21,11 +21,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -136,6 +132,4 @@ void printCSCMatrix(const CSC_Matrix & csc_mat) std::cout << "]\n"; } -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp similarity index 98% rename from common/osqp_interface/src/osqp_interface.cpp rename to common/autoware_osqp_interface/src/osqp_interface.cpp index f0dbc3ab22e4a..9ebf132f65028 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/autoware_osqp_interface/src/osqp_interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include "osqp/osqp.h" -#include "osqp_interface/csc_matrix_conv.hpp" #include #include @@ -25,11 +25,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) : m_work{nullptr, OSQPWorkspaceDeleter} @@ -436,6 +432,4 @@ void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const // log with warning RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); } -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface diff --git a/common/osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp similarity index 93% rename from common/osqp_interface/test/test_csc_matrix_conv.cpp rename to common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp index 765f1a1afed3b..a63f979a966bf 100644 --- a/common/osqp_interface/test/test_csc_matrix_conv.cpp +++ b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include "gtest/gtest.h" -#include "osqp_interface/csc_matrix_conv.hpp" #include @@ -23,8 +23,8 @@ TEST(TestCscMatrixConv, Nominal) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; @@ -117,8 +117,8 @@ TEST(TestCscMatrixConv, Nominal) } TEST(TestCscMatrixConv, Trapezoidal) { - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; - using autoware::common::osqp::printCSCMatrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; + using autoware::osqp_interface::printCSCMatrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd rect1(1, 2); square1 << 1.0, 2.0, 2.0, 4.0; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp similarity index 85% rename from common/osqp_interface/test/test_osqp_interface.cpp rename to common/autoware_osqp_interface/test/test_osqp_interface.cpp index 4644ec6c3e019..d03713f82566f 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/osqp_interface/osqp_interface.hpp" #include "gtest/gtest.h" -#include "osqp_interface/osqp_interface.hpp" #include @@ -39,9 +39,9 @@ namespace TEST(TestOsqpInterface, BasicQp) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; auto check_result = [](const std::tuple, std::vector, int, int, int> & result) { @@ -66,12 +66,12 @@ TEST(TestOsqpInterface, BasicQp) const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; { // Define problem during optimization - autoware::common::osqp::OSQPInterface osqp; + autoware::osqp_interface::OSQPInterface osqp; std::tuple, std::vector, int, int, int> result = osqp.optimize(P, A, q, l, u); check_result(result); @@ -79,7 +79,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during initialization - autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6); std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } @@ -92,7 +92,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization @@ -105,7 +105,7 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } @@ -118,7 +118,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization @@ -138,7 +138,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 1e1295cb06b1e..ab9b3392449ee 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" namespace autoware::motion::control::mpc_lateral_controller @@ -58,7 +58,7 @@ class QPSolverOSQP : public QPSolverInterface double getObjVal() const override { return osqpsolver_.getObjVal(); } private: - autoware::common::osqp::OSQPInterface osqpsolver_; + autoware::osqp_interface::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 5c5f8886d6ed3..ee9ede3598256 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -20,6 +20,7 @@ autoware_control_msgs autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_trajectory_follower_base autoware_universe_utils @@ -29,7 +30,6 @@ diagnostic_updater eigen geometry_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index 548129eb06f8c..c4b6fbfeb4ca6 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -23,6 +23,7 @@ autoware_control_msgs autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -31,7 +32,6 @@ diagnostic_updater eigen geometry_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp index a2ac86ee6c86b..ca17b428e369a 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp @@ -15,7 +15,7 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ #include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include @@ -69,7 +69,7 @@ class VelocityOptimizer double over_j_weight_; // QPSolver - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; }; #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 90a5e666f5f44..ed3cd2169e343 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -22,6 +22,7 @@ autoware_interpolation autoware_lanelet2_extension autoware_motion_utils + autoware_osqp_interface autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager @@ -31,7 +32,6 @@ geometry_msgs nav_msgs object_recognition_utils - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3d9192c93d5b1..9a397e255da13 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -17,6 +17,7 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" @@ -24,7 +25,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" -#include "osqp_interface/osqp_interface.hpp" #include #include @@ -228,7 +228,7 @@ class MPTOptimizer MPTParam mpt_param_; StateEquationGenerator state_equation_generator_; - std::unique_ptr osqp_solver_ptr_; + std::unique_ptr osqp_solver_ptr_; const double osqp_epsilon_ = 1.0e-3; diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 1a7869b6a87fd..cdd7123d21ab8 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -16,13 +16,13 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 8b48f78d117bb..df07f3f98957a 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -414,7 +414,7 @@ MPTOptimizer::MPTOptimizer( StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver - osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); + osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); // publisher debug_fixed_traj_pub_ = node->create_publisher("~/debug/mpt_fixed_traj", 1); @@ -1330,8 +1330,8 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // NOTE: The following takes 1 [ms] Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, N_v); - Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); - Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); + Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::osqp_interface::INF); + Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::osqp_interface::INF); size_t A_rows_end = 0; // 1. State equation @@ -1502,9 +1502,9 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // initialize or update solver according to warm start time_keeper_->start_track("initOsqp"); - const autoware::common::osqp::CSC_Matrix P_csc = - autoware::common::osqp::calCSCMatrixTrapezoidal(H); - const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); + const autoware::osqp_interface::CSC_Matrix P_csc = + autoware::osqp_interface::calCSCMatrixTrapezoidal(H); + const autoware::osqp_interface::CSC_Matrix A_csc = autoware::osqp_interface::calCSCMatrix(A); if ( prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { @@ -1515,7 +1515,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); } else { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); - osqp_solver_ptr_ = std::make_unique( + osqp_solver_ptr_ = std::make_unique( P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); } prev_mat_n_ = H.rows(); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp index 1d0441c66c481..2d6c1d1b2d002 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ #define AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "osqp_interface/osqp_interface.hpp" #include @@ -109,7 +109,7 @@ class EBPathSmoother rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; rclcpp::Publisher::SharedPtr debug_eb_fixed_traj_pub_; - std::unique_ptr osqp_solver_ptr_; + std::unique_ptr osqp_solver_ptr_; std::shared_ptr> prev_eb_traj_points_ptr_{nullptr}; std::vector insertFixedPoint( diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index b9b79bb6ceaf6..582553c2a94b2 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -16,12 +16,12 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs nav_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 4222e0fe98438..0bec4e46c8b42 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -381,7 +381,7 @@ void EBPathSmoother::updateConstraint( osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); } else { - osqp_solver_ptr_ = std::make_unique( + osqp_solver_ptr_ = std::make_unique( P, A, q, lower_bound, upper_bound, p.qp_param.eps_abs); osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); osqp_solver_ptr_->updateEpsAbs(p.qp_param.eps_abs); diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 2abae4af2ca67..4f1399533ed46 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -23,6 +23,7 @@ autoware_map_projection_loader autoware_mission_planner autoware_motion_utils + autoware_osqp_interface autoware_path_optimizer autoware_path_smoother autoware_perception_msgs @@ -33,7 +34,6 @@ geometry_msgs global_parameter_loader map_loader - osqp_interface rclcpp rclcpp_components tier4_map_msgs diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index d6033b66d5d6f..3f1c313e052da 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" @@ -31,7 +32,6 @@ #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 8f6bbc2236eb6..fda8e8f31df95 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class L2PseudoJerkSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index e27a4db10e748..119cb4d358de2 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class LinfPseudoJerkSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 65684e414e90d..222b189057820 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -23,13 +23,13 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs - osqp_interface qp_interface rclcpp tf2 From 0de8669e2d112b1b51edc38b831a3e07963993b7 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 15 Oct 2024 10:32:52 +0900 Subject: [PATCH 18/27] test(no_stopping_area): refactor and add tests (#9009) Signed-off-by: Maxime CLEMENT Co-authored-by: Kosuke Takeuchi --- .../CMakeLists.txt | 13 +- .../package.xml | 8 +- .../src/debug.cpp | 35 +- .../src/manager.cpp | 5 - .../src/manager.hpp | 2 +- .../src/scene_no_stopping_area.cpp | 295 ++---------- .../src/scene_no_stopping_area.hpp | 92 +--- .../src/utils.cpp | 262 +++++++++++ .../src/utils.hpp | 165 +++++++ .../test/test_utils.cpp | 438 ++++++++++++++++++ .../scene_module_interface.hpp | 2 +- 11 files changed, 936 insertions(+), 381 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt index f4646b1b9eb6e..b710924410549 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -6,9 +6,16 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/manager.cpp - src/scene_no_stopping_area.cpp + 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_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 21664f7596d60..18a35ea12d5f6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -8,6 +8,7 @@ Kosuke Takeuchi Tomoya Kimura Shumpei Wakabayashi + Shumpei Wakabayashi Apache License 2.0 @@ -15,7 +16,6 @@ ament_cmake_auto autoware_cmake - eigen3_cmake_module autoware_behavior_velocity_planner_common autoware_interpolation @@ -23,20 +23,16 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils - eigen geometry_msgs libboost-dev pluginlib rclcpp - tf2 - tf2_eigen - tf2_geometry_msgs tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index 09f1908b107d5..b9509e308d1fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -13,17 +13,13 @@ // limitations under the License. #include "scene_no_stopping_area.hpp" +#include "utils.hpp" #include #include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif #include #include @@ -33,13 +29,13 @@ namespace autoware::behavior_velocity_planner namespace { const double marker_lifetime = 0.2; -using DebugData = NoStoppingAreaModule::DebugData; +using DebugData = no_stopping_area::DebugData; using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; using autoware::universe_utils::createMarkerScale; -lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) +lanelet::BasicPoint3d get_centroid_point(const lanelet::BasicPolygon3d & poly) { lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0}; for (const auto & p : poly) { @@ -48,7 +44,7 @@ lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) return p_sum / poly.size(); } -geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) +geometry_msgs::msg::Point to_msg(const lanelet::BasicPoint3d & point) { geometry_msgs::msg::Point msg; msg.x = point.x(); @@ -57,7 +53,7 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) return msg; } -visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( +visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now) { visualization_msgs::msg::MarkerArray msg; @@ -65,7 +61,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( // ID { auto marker = createDefaultMarker( - "map", now, "no_stopping_area_id", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_id", static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); @@ -73,7 +69,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); - marker.pose.position = toMsg(poly.front()); + marker.pose.position = to_msg(poly.front()); marker.pose.position.z += 2.0; marker.text = std::to_string(no_stopping_area_reg_elem.id()); @@ -84,7 +80,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( // Polygon { auto marker = createDefaultMarker( - "map", now, "no_stopping_area_polygon", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_polygon", static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); @@ -99,8 +95,8 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const auto & p_front = poly.at(idx_front); const auto & p_back = poly.at(idx_back); - marker.points.push_back(toMsg(p_front)); - marker.points.push_back(toMsg(p_back)); + marker.points.push_back(to_msg(p_front)); + marker.points.push_back(to_msg(p_back)); } } msg.markers.push_back(marker); @@ -112,16 +108,17 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const auto stop_line_center_point = (stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2; auto marker = createDefaultMarker( - "map", now, "no_stopping_area_correspondence", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_correspondence", + static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); - const auto centroid_point = getCentroidPoint(poly); + const auto centroid_point = get_centroid_point(poly); for (size_t i = 0; i < poly.size(); ++i) { - marker.points.push_back(toMsg(centroid_point)); - marker.points.push_back(toMsg(stop_line_center_point)); + marker.points.push_back(to_msg(centroid_point)); + marker.points.push_back(to_msg(stop_line_center_point)); } } msg.markers.push_back(marker); @@ -137,7 +134,7 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra const rclcpp::Time now = clock_->now(); appendMarkerArray( - createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, now), &debug_marker_array, now); + create_lanelet_info_marker_array(no_stopping_area_reg_elem_, now), &debug_marker_array, now); if (!debug_data_.stuck_points.empty()) { appendMarkerArray( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 4631195ec47fc..9d66aa6672d36 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -16,17 +16,12 @@ #include #include -#include - -#include #include #include #include #include -#include #include -#include namespace autoware::behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 1abd36d585d51..7009e94612580 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -37,7 +37,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "no_stopping_area"; } private: - NoStoppingAreaModule::PlannerParam planner_param_; + NoStoppingAreaModule::PlannerParam planner_param_{}; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index ac58c62036e26..031f2976da0a7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -14,26 +14,22 @@ #include "scene_no_stopping_area.hpp" -#include -#include +#include "utils.hpp" + +#include #include #include #include #include #include +#include +#include #include +#include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include -#include -#include -#include +#include #include namespace autoware::behavior_velocity_planner @@ -43,7 +39,7 @@ namespace bg = boost::geometry; NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), @@ -56,55 +52,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( state_machine_.setMarginTime(planner_param_.state_clear_time); } -boost::optional NoStoppingAreaModule::getStopLineGeometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const -{ - // get stop line from map - { - const auto & stop_line = no_stopping_area_reg_elem_.stopLine(); - if (stop_line) { - return planning_utils::extendLine( - stop_line.value()[0], stop_line.value()[1], planner_data_->stop_line_extend_length); - } - } - // auto gen stop line - { - LineString2d stop_line; - /** - * @brief auto gen no stopping area stop line from area polygon if stop line is not set - * --------------- - * ------col-------------|--> ego path - * | Area | - * --------------- - **/ - - for (const auto & no_stopping_area : no_stopping_area_reg_elem_.noStoppingAreas()) { - const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); - lanelet::BasicLineString2d path_line; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto p0 = path.points.at(i).point.pose.position; - const auto p1 = path.points.at(i + 1).point.pose.position; - const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; - std::vector collision_points; - bg::intersection(area_poly, line, collision_points); - if (!collision_points.empty()) { - const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); - const double w = planner_data_->vehicle_info_.vehicle_width_m; - const double l = stop_line_margin; - stop_line.emplace_back( - -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), - collision_points.front().y() + w * std::sin(yaw + M_PI_2)); - stop_line.emplace_back( - -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), - collision_points.front().y() + w * std::sin(yaw - M_PI_2)); - return stop_line; - } - } - } - } - return {}; -} - bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { // Store original path @@ -115,12 +62,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } // Reset data - debug_data_ = DebugData(); + debug_data_ = no_stopping_area::DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; *stop_reason = planning_utils::initializeStopReason(StopReason::NO_STOPPING_AREA); + const no_stopping_area::EgoData ego_data(*planner_data_); + // Get stop line geometry - const auto stop_line = getStopLineGeometry2d(original_path, planner_param_.stop_line_margin); + const auto stop_line = no_stopping_area::get_stop_line_geometry2d( + original_path, no_stopping_area_reg_elem_, planner_param_.stop_line_margin, + planner_data_->stop_line_extend_length, planner_data_->vehicle_info_.vehicle_width_m); if (!stop_line) { setSafe(true); return true; @@ -146,14 +97,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason const double margin = planner_param_.stop_line_margin; const double ego_space_in_front_of_stuck_vehicle = margin + vi.vehicle_length_m + planner_param_.stuck_vehicle_front_margin; - const Polygon2d stuck_vehicle_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose->pose, ego_space_in_front_of_stuck_vehicle, - planner_param_.detection_area_length); + const Polygon2d stuck_vehicle_detect_area = + no_stopping_area::generate_ego_no_stopping_area_lane_polygon( + *path, current_pose->pose, no_stopping_area_reg_elem_, ego_space_in_front_of_stuck_vehicle, + planner_param_.detection_area_length, planner_param_.path_expand_width, logger_, *clock_); const double ego_space_in_front_of_stop_line = margin + planner_param_.stop_margin + vi.rear_overhang_m; - const Polygon2d stop_line_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose->pose, ego_space_in_front_of_stop_line, - planner_param_.detection_area_length); + const Polygon2d stop_line_detect_area = + no_stopping_area::generate_ego_no_stopping_area_lane_polygon( + *path, current_pose->pose, no_stopping_area_reg_elem_, ego_space_in_front_of_stop_line, + planner_param_.detection_area_length, planner_param_.path_expand_width, logger_, *clock_); if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) { setSafe(true); return true; @@ -162,26 +115,28 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area); // Find stuck vehicle in no stopping area const bool is_entry_prohibited_by_stuck_vehicle = - checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr); + check_stuck_vehicles_in_no_stopping_area(stuck_vehicle_detect_area, predicted_obj_arr_ptr); // Find stop line in no stopping area const bool is_entry_prohibited_by_stop_line = - checkStopLinesInNoStoppingArea(*path, stop_line_detect_area); + no_stopping_area::check_stop_lines_in_no_stopping_area( + *path, stop_line_detect_area, debug_data_); const bool is_entry_prohibited = is_entry_prohibited_by_stuck_vehicle || is_entry_prohibited_by_stop_line; - if (!isStoppable(current_pose->pose, stop_point->second)) { + if (!no_stopping_area::is_stoppable( + pass_judge_, current_pose->pose, stop_point->second, ego_data, logger_, *clock_)) { state_machine_.setState(StateMachine::State::GO); setSafe(true); return false; - } else { - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); } + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + setSafe(state_machine_.getState() != StateMachine::State::STOP); if (!isActivated()) { // ----------------stop reason and stop point-------------------------- - insertStopPoint(*path, *stop_point); + no_stopping_area::insert_stop_point(*path, *stop_point); // For virtual wall debug_data_.stop_poses.push_back(stop_pose); @@ -208,19 +163,19 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason } } else if (state_machine_.getState() == StateMachine::State::GO) { // reset pass judge if current state is go - is_stoppable_ = true; - pass_judged_ = false; + pass_judge_.is_stoppable = true; + pass_judge_.pass_judged = false; } return true; } -bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( +bool NoStoppingAreaModule::check_stuck_vehicles_in_no_stopping_area( const Polygon2d & poly, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { + if (!no_stopping_area::is_vehicle_type(object)) { continue; // not target vehicle type } const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); @@ -248,182 +203,4 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( } return false; } -bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) -{ - const double stop_vel = std::numeric_limits::min(); - - // if the detected stop point is near goal, it's ignored. - static constexpr double close_to_goal_distance = 1.0; - - // stuck points by stop line - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto p0 = path.points.at(i).point.pose.position; - const auto p1 = path.points.at(i + 1).point.pose.position; - const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; - const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; - if (v0 > stop_vel && v1 > stop_vel) { - continue; - } - // judge if stop point p0 is near goal, by its distance to the path end. - const double dist_to_path_end = - autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); - if (dist_to_path_end < close_to_goal_distance) { - // exit with false, cause position is near goal. - return false; - } - - const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; - std::vector collision_points; - bg::intersection(poly, line, collision_points); - if (!collision_points.empty()) { - geometry_msgs::msg::Point point; - point.x = collision_points.front().x(); - point.y = collision_points.front().y(); - point.z = 0.0; - debug_data_.stuck_points.emplace_back(point); - return true; - } - } - return false; -} - -Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const double margin, const double extra_dist) const -{ - Polygon2d ego_area; // open polygon - double dist_from_start_sum = 0.0; - const double interpolation_interval = 0.5; - bool is_in_area = false; - tier4_planning_msgs::msg::PathWithLaneId interpolated_path; - if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { - return ego_area; - } - auto & pp = interpolated_path.points; - /* calc closest index */ - const auto closest_idx_opt = - autoware::motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); - if (!closest_idx_opt) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); - return ego_area; - } - const size_t closest_idx = closest_idx_opt.value(); - - const int num_ignore_nearest = 1; // Do not consider nearest lane polygon - size_t ego_area_start_idx = closest_idx + num_ignore_nearest; - // return if area size is not intentional - if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) { - return ego_area; - } - const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); - for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - const auto & p = pp.at(i).point.pose.position; - if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - is_in_area = true; - break; - } - if (dist_from_start_sum > extra_dist) { - return ego_area; - } - ++ego_area_start_idx; - } - if (ego_area_start_idx > num_ignore_nearest) { - ego_area_start_idx--; - } - if (!is_in_area) { - return ego_area; - } - double dist_from_area_sum = 0.0; - // decide end idx with extract distance - size_t ego_area_end_idx = ego_area_start_idx; - for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - const auto & p = pp.at(i).point.pose.position; - if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - } - if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { - break; - } - ++ego_area_end_idx; - } - - const auto width = planner_param_.path_expand_width; - ego_area = planning_utils::generatePathPolygon( - interpolated_path, ego_area_start_idx, ego_area_end_idx, width); - return ego_area; -} - -bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) const -{ - if ( - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::CAR || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::BUS || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool NoStoppingAreaModule::isStoppable( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const -{ - // get vehicle info and compute pass_judge_line_distance - const auto current_velocity = planner_data_->current_velocity->twist.linear.x; - const auto current_acceleration = planner_data_->current_acceleration->accel.accel.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double max_jerk = planner_data_->max_stop_jerk_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( - current_velocity, current_acceleration, max_acc, max_jerk, delay_response_time); - const double signed_arc_length = - arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); - const bool distance_stoppable = stoppable_distance < signed_arc_length; - const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; - // ego vehicle is high speed and can't stop before stop line -> GO - const bool not_stoppable = !distance_stoppable && !slow_velocity; - // stoppable or not is judged only once - RCLCPP_DEBUG( - logger_, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); - if (!distance_stoppable && !pass_judged_) { - pass_judged_ = true; - // can't stop using maximum brake consider jerk limit - if (not_stoppable) { - // pass through - is_stoppable_ = false; - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); - } else { - is_stoppable_ = true; - } - } - return is_stoppable_; -} - -void NoStoppingAreaModule::insertStopPoint( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) -{ - size_t insert_idx = static_cast(stop_point.first + 1); - const auto stop_pose = stop_point.second; - - // To PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; - stop_point_with_lane_id = path.points.at(insert_idx); - stop_point_with_lane_id.point.pose = stop_pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); -} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 2c553ac457ef0..886c8f04dc702 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -15,48 +15,25 @@ #ifndef SCENE_NO_STOPPING_AREA_HPP_ #define SCENE_NO_STOPPING_AREA_HPP_ -#define EIGEN_MPL2_ONLY +#include "utils.hpp" -#include #include #include #include #include #include -#include #include #include #include -#include - -#include -#include - #include -#include -#include namespace autoware::behavior_velocity_planner { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d -using PathIndexWithOffset = std::pair; // front index, offset - class NoStoppingAreaModule : public SceneModuleInterface { public: - struct DebugData - { - double base_link2front; - std::vector stop_poses; - geometry_msgs::msg::Pose first_stop_pose; - std::vector stuck_points; - geometry_msgs::msg::Polygon stuck_vehicle_detect_area; - geometry_msgs::msg::Polygon stop_line_detect_area; - }; - struct PlannerParam { /** @@ -76,11 +53,10 @@ class NoStoppingAreaModule : public SceneModuleInterface double path_expand_width; //! [m] path width to calculate the edge line for both side }; -public: NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -91,77 +67,19 @@ class NoStoppingAreaModule : public SceneModuleInterface private: const int64_t lane_id_; - mutable bool pass_judged_ = false; - mutable bool is_stoppable_ = true; + no_stopping_area::PassJudge pass_judge_; StateMachine state_machine_; //! for state - /** - * @brief check if the object has a target type for stuck check - * @param object target object - * @return true if the object has a target type - */ - bool isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) const; - /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. * @param poly ego focusing area polygon * @param objects_ptr target objects * @return true if exists */ - bool checkStuckVehiclesInNoStoppingArea( + bool check_stuck_vehicles_in_no_stopping_area( const Polygon2d & poly, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); - /** - * @brief Check if there is a stop line in "stop line detect area". - * @param path ego-car lane - * @param poly ego focusing area polygon - * @return true if exists - */ - bool checkStopLinesInNoStoppingArea( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); - - /** - * @brief Calculate the polygon of the path from the ego-car position to the end of the - * no stopping lanelet (+ extra distance). - * @param path ego-car lane - * @param ego_pose ego-car pose - * @param margin margin from the end point of the ego-no stopping area lane - * @param extra_dist extra distance from the end point of the no stopping area lanelet - * @return generated polygon - */ - Polygon2d generateEgoNoStoppingAreaLanePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; - - /** - * @brief Calculate the polygon of the path from the ego-car position to the end of the - * no stopping lanelet (+ extra distance). - * @param path ego-car lane - * @param stop_line_margin stop line margin from the stopping area lane - * @return generated stop line - */ - boost::optional getStopLineGeometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const; - - /** - * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit - * @param self_pose ego-car pose - * @param line_pose stop line pose on the lane - * @return is stoppable in front of no stopping area - */ - bool isStoppable( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; - - /** - * @brief insert stop point on ego path - * @param path original path - * @param stop_point stop line point on the lane - */ - void insertStopPoint( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); - // Key Feature const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; std::shared_ptr last_obstacle_found_time_; @@ -170,7 +88,7 @@ class NoStoppingAreaModule : public SceneModuleInterface PlannerParam planner_param_; // Debug - DebugData debug_data_; + no_stopping_area::DebugData debug_data_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp new file mode 100644 index 0000000000000..5159233c341ed --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -0,0 +1,262 @@ +// 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 "utils.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::behavior_velocity_planner::no_stopping_area +{ + +bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & object) +{ + // TODO(anyone): should we switch to using functions from the common object_recognition_utils ? + return !object.classification.empty() && + (object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::CAR || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::BUS || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); +} + +void insert_stop_point( + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) +{ + auto insert_idx = stop_point.first + 1UL; + const auto & stop_pose = stop_point.second; + + // To PathPointWithLaneId + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = path.points.at(insert_idx); + stop_point_with_lane_id.point.pose = stop_pose; + stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Insert stop point or replace with zero velocity + planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); +} + +std::optional generate_stop_line( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, + const double stop_line_margin) +{ + LineString2d stop_line; + for (const auto & no_stopping_area : no_stopping_areas) { + const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + boost::geometry::intersection(area_poly, line, collision_points); + if (!collision_points.empty()) { + const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); + const double w = ego_width; + const double l = stop_line_margin; + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), + collision_points.front().y() + w * std::sin(yaw + M_PI_2)); + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), + collision_points.front().y() + w * std::sin(yaw - M_PI_2)); + return stop_line; + } + } + } + return {}; +} + +bool is_stoppable( + PassJudge & pass_judge, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const EgoData & ego_data, + const rclcpp::Logger & logger, rclcpp::Clock & clock) +{ + // compute pass_judge_line_distance + const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( + ego_data.current_velocity, ego_data.current_acceleration, ego_data.max_stop_acc, + ego_data.max_stop_jerk, ego_data.delay_response_time); + const double signed_arc_length = + arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); + const bool distance_stoppable = stoppable_distance < signed_arc_length; + const bool slow_velocity = ego_data.current_velocity < 2.0; + // ego vehicle is high speed and can't stop before stop line -> GO + const bool not_stoppable = !distance_stoppable && !slow_velocity; + // stoppable or not is judged only once + RCLCPP_DEBUG( + logger, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); + if (!distance_stoppable && !pass_judge.pass_judged) { + pass_judge.pass_judged = true; + // can't stop using maximum brake consider jerk limit + if (not_stoppable) { // TODO(someone): this can be replaced by !slow_velocity, is this correct? + // pass through + pass_judge.is_stoppable = false; + RCLCPP_WARN_THROTTLE( + logger, clock, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); + } else { + pass_judge.is_stoppable = true; + } + } + return pass_judge.is_stoppable; +} + +Polygon2d generate_ego_no_stopping_area_lane_polygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, + const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, + rclcpp::Clock & clock) +{ + Polygon2d ego_area; // open polygon + double dist_from_start_sum = 0.0; + constexpr double interpolation_interval = 0.5; + bool is_in_area = false; + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger)) { + return ego_area; + } + const auto & pp = interpolated_path.points; + /* calc closest index */ + const auto closest_idx_opt = autoware::motion_utils::findNearestIndex(pp, ego_pose, 3.0, M_PI_4); + if (!closest_idx_opt) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, clock, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); + return ego_area; + } + const size_t closest_idx = closest_idx_opt.value(); + + const int num_ignore_nearest = 1; // Do not consider nearest lane polygon + size_t ego_area_start_idx = closest_idx + num_ignore_nearest; + // return if area size is not intentional + if (no_stopping_area_reg_elem.noStoppingAreas().size() != 1) { + return ego_area; + } + const auto no_stopping_area = no_stopping_area_reg_elem.noStoppingAreas().front(); + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + // TODO(someone): within will skip points on the edge of polygons so some points can be skipped + // depending on the interpolation + if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + is_in_area = true; + break; + } + if (dist_from_start_sum > max_polygon_length) { + return ego_area; + } + ++ego_area_start_idx; + } + if (ego_area_start_idx > num_ignore_nearest) { + // TODO(someone): check if this is a bug + // this -1 causes pp[ego_area_start_idx] to be outside the no_stopping_area + // it causes "dist_from_area" to count the first point in the next loop + // moreover it causes the "dist_from_start_sum" to count the same point twice + ego_area_start_idx--; + } + if (!is_in_area) { + return ego_area; + } + double dist_from_area_sum = 0.0; + // decide end idx with extract distance + size_t ego_area_end_idx = ego_area_start_idx; + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + // TODO(someone): within will skip points on the edge of polygons so some points can be skipped + // depending on the interpolation + if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + } + if (dist_from_start_sum > max_polygon_length || dist_from_area_sum > margin) { + break; + } + ++ego_area_end_idx; + } + + ego_area = planning_utils::generatePathPolygon( + interpolated_path, ego_area_start_idx, ego_area_end_idx, path_expand_width); + return ego_area; +} + +bool check_stop_lines_in_no_stopping_area( + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + no_stopping_area::DebugData & debug_data) +{ + const double stop_vel = std::numeric_limits::min(); + + // if the detected stop point is near goal, it's ignored. + static constexpr double close_to_goal_distance = 1.0; + + // stuck points by stop line + for (size_t i = 0; i + 1 < path.points.size(); ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; + const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; + if (v0 > stop_vel && v1 > stop_vel) { + continue; + } + // judge if stop point p0 is near goal, by its distance to the path end. + const double dist_to_path_end = + autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); + if (dist_to_path_end < close_to_goal_distance) { + // exit with false, cause position is near goal. + return false; + } + + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(poly, line, collision_points); + if (!collision_points.empty()) { + geometry_msgs::msg::Point point; + point.x = collision_points.front().x(); + point.y = collision_points.front().y(); + point.z = 0.0; + debug_data.stuck_points.emplace_back(point); + return true; + } + } + return false; +} + +std::optional get_stop_line_geometry2d( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width) +{ + const auto & stop_line = no_stopping_area_reg_elem.stopLine(); + if (stop_line && stop_line->size() >= 2) { + // get stop line from map + return planning_utils::extendLine( + stop_line.value()[0], stop_line.value()[1], stop_line_extend_length); + } + return generate_stop_line( + path, no_stopping_area_reg_elem.noStoppingAreas(), vehicle_width, stop_line_margin); +} + +} // namespace autoware::behavior_velocity_planner::no_stopping_area diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp new file mode 100644 index 0000000000000..73b835b4701dc --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp @@ -0,0 +1,165 @@ +// 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner::no_stopping_area +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +struct PassJudge +{ + bool pass_judged = false; + bool is_stoppable = true; +}; + +struct DebugData +{ + double base_link2front; + std::vector stop_poses; + geometry_msgs::msg::Pose first_stop_pose; + std::vector stuck_points; + geometry_msgs::msg::Polygon stuck_vehicle_detect_area; + geometry_msgs::msg::Polygon stop_line_detect_area; +}; + +// intermediate data about the ego vehicle taken from the PlannerData +struct EgoData +{ + EgoData() = default; + explicit EgoData(const PlannerData & planner_data) + { + current_velocity = planner_data.current_velocity->twist.linear.x; + current_acceleration = planner_data.current_acceleration->accel.accel.linear.x; + max_stop_acc = planner_data.max_stop_acceleration_threshold; + max_stop_jerk = planner_data.max_stop_jerk_threshold; + delay_response_time = planner_data.delay_response_time; + } + + double current_velocity{}; + double current_acceleration{}; + double max_stop_acc{}; + double max_stop_jerk{}; + double delay_response_time{}; +}; + +/** + * @brief check if the object is a vehicle (car, bus, truck, trailer, motorcycle) + * @param object input object + * @return true if the object is a vehicle + */ +bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & object); + +/** + * @brief insert stop point on ego path + * @param path original path + * @param stop_point stop line point on the lane + */ +void insert_stop_point( + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + +/** + * @brief generate stop line from no stopping area polygons + * ________________ + * ------|--|--------------|--> ego path + * stop | | Area | + * line | L______________/ + * @param path input path + * @param no_stopping_areas no stopping area polygons + * @param ego_width [m] width of ego + * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas + **/ +std::optional generate_stop_line( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, + const double stop_line_margin); + +/** + * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit + * @param [inout] pass_judge the pass judge decision to update + * @param self_pose ego-car pose + * @param line_pose stop line pose on the lane + * @param ego_data planner data with ego pose, velocity, etc + * @param logger ros logger + * @param clock ros clock + * @return is stoppable in front of no stopping area + */ +bool is_stoppable( + PassJudge & pass_judge, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const EgoData & ego_data, + const rclcpp::Logger & logger, rclcpp::Clock & clock); + +/** + * @brief Calculate the polygon of the path from the ego-car position to the end of the + * no stopping lanelet (+ extra distance). + * @param path ego-car lane + * @param ego_pose ego-car pose + * @param margin margin from the end point of the ego-no stopping area lane + * @param max_polygon_length maximum length of the polygon + * @return generated polygon + */ +Polygon2d generate_ego_no_stopping_area_lane_polygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, + const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, + rclcpp::Clock & clock); + +/** + * @brief Check if there is a stop line in "stop line detect area". + * @param path ego-car lane + * @param poly ego focusing area polygon + * @param [out] debug_data structure to store the stuck points for debugging + * @return true if exists + */ +bool check_stop_lines_in_no_stopping_area( + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + DebugData & debug_data); + +/** + * @brief Calculate the stop line of a no stopping area + * @details use the stop line of the regulatory element if it exists, otherwise generate it + * @param path ego path + * @param no_stopping_area_reg_elem no_stopping_area regulatory element + * @param stop_line_margin [m] margin between the stop line and the start of the no stopping area + * @param stop_line_extend_length [m] extra length to add on each side of the stop line (only added + * to the stop line of the regulatory element) + * @param vehicle_width [m] width of the ego vehicle + * @return generated stop line + */ +std::optional get_stop_line_geometry2d( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width); + +} // namespace autoware::behavior_velocity_planner::no_stopping_area + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp new file mode 100644 index 0000000000000..cc5bbd2903a52 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp @@ -0,0 +1,438 @@ +// 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/utils.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +template +bool point_in_polygon(const Point & p, const Polygon & poly) +{ + return std::find_if(poly.outer().begin(), poly.outer().end(), [&](const auto & o) { + return p.x() == o.x() && p.y() == o.y(); + }) != poly.outer().end(); +} + +tier4_planning_msgs::msg::PathWithLaneId generate_straight_path( + const size_t nb_points, const float velocity = 0.0, const double resolution = 1.0) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + for (auto x = 0UL; x < nb_points; ++x) { + tier4_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = resolution * static_cast(x); + p.point.pose.position.y = 0.0; + p.point.longitudinal_velocity_mps = velocity; + path.points.push_back(p); + } + return path; +} + +TEST(NoStoppingAreaTest, isTargetStuckVehicleType) +{ + using autoware::behavior_velocity_planner::no_stopping_area::is_vehicle_type; + autoware_perception_msgs::msg::PredictedObject object; + EXPECT_NO_FATAL_FAILURE(is_vehicle_type(object)); + autoware_perception_msgs::msg::ObjectClassification classification; + for (const auto label : { + autoware_perception_msgs::msg::ObjectClassification::CAR, + autoware_perception_msgs::msg::ObjectClassification::BUS, + autoware_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_perception_msgs::msg::ObjectClassification::TRAILER, + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + }) { + classification.label = label; + object.classification = {classification}; + EXPECT_TRUE(is_vehicle_type(object)); + } +} + +TEST(NoStoppingAreaTest, insertStopPoint) +{ + using autoware::behavior_velocity_planner::no_stopping_area::insert_stop_point; + constexpr auto nb_points = 10; + constexpr auto default_velocity = 5.0; + const tier4_planning_msgs::msg::PathWithLaneId base_path = + generate_straight_path(nb_points, default_velocity); + autoware::behavior_velocity_planner::no_stopping_area::PathIndexWithPose stop_point; + // stop exactly at a point, expect no new points but a 0 velocity at the stop point and after + auto path = base_path; + stop_point.first = 4UL; + stop_point.second = path.points[stop_point.first].point.pose; + insert_stop_point(path, stop_point); + ASSERT_EQ(path.points.size(), nb_points); + for (auto i = 0UL; i < stop_point.first; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, default_velocity); + } + for (auto i = stop_point.first; i < nb_points; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, 0.0); + } + + // stop between points + path = base_path; + stop_point.first = 3UL; + stop_point.second.position.x = 3.5; + insert_stop_point(path, stop_point); + ASSERT_EQ(path.points.size(), nb_points + 1); + EXPECT_EQ(path.points[stop_point.first + 1].point.pose.position.x, stop_point.second.position.x); + for (auto i = 0UL; i <= stop_point.first; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, default_velocity); + } + for (auto i = stop_point.first + 1; i < nb_points + 1; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, 0.0); + } + + // stop at the last point: exception // TODO(anyone): is this okay ? + path = base_path; + stop_point.first = path.points.size() - 1; + stop_point.second = path.points.back().point.pose; + EXPECT_THROW(insert_stop_point(path, stop_point), std::out_of_range); +} + +TEST(NoStoppingAreaTest, generateStopLine) +{ + using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; + constexpr auto nb_points = 10; + const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(nb_points); + lanelet::ConstPolygons3d no_stopping_areas; + double ego_width = 0.0; + double stop_line_margin = 0.0; + // no areas, expect no stop line + auto stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_FALSE(stop_line.has_value()); + + // area outside of the path + lanelet::Polygon3d poly; + poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -2.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -0.5)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -0.5)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -2.0)); + no_stopping_areas.push_back(poly); + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_FALSE(stop_line.has_value()); + // area on the path, 0 margin and 0 width + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, 1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, -1.0)); + no_stopping_areas.push_back(poly); + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), 5.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 5.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + // set a margin + stop_line_margin = 1.0; + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), 4.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 4.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + // set a width + ego_width = 2.0; + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + // TODO(anyone): if we stop at this stop line ego overlaps the 1st area, is this okay ? + EXPECT_EQ(stop_line->front().x(), 4.0); + EXPECT_EQ(stop_line->front().y(), 2.0); + EXPECT_EQ(stop_line->back().x(), 4.0); + EXPECT_EQ(stop_line->back().y(), -2.0); +} + +TEST(NoStoppingAreaTest, isStoppable) +{ + using autoware::behavior_velocity_planner::no_stopping_area::is_stoppable; + autoware::behavior_velocity_planner::no_stopping_area::PassJudge pass_judge; + geometry_msgs::msg::Pose self_pose; + geometry_msgs::msg::Pose line_pose; + rclcpp::Clock clock; + auto logger = rclcpp::get_logger("test_logger"); + logger.set_level(rclcpp::Logger::Level::Debug); + autoware::behavior_velocity_planner::no_stopping_area::EgoData ego_data; + ego_data.current_velocity = 1.0; + ego_data.current_acceleration = 0.0; + ego_data.delay_response_time = 0.0; + ego_data.max_stop_acc = -1.0; + ego_data.max_stop_jerk = -1.0; + // try to stop 1m ahead + self_pose.position.x = 0.0; + self_pose.position.y = 0.0; + line_pose.position.x = 1.0; + line_pose.position.y = 0.0; + + // enough distance to stop and slow velocity + EXPECT_TRUE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_TRUE(pass_judge.is_stoppable); + EXPECT_FALSE(pass_judge.pass_judged); + + // unstoppable and fast velocity + ego_data.current_velocity = 10.0; + EXPECT_FALSE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_FALSE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); + + // unstoppable and slow velocity, but since we already judged, it is not updated + ego_data.current_velocity = 1.9; + EXPECT_FALSE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_FALSE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); + + // reset and result is updated + // TODO(someone): is this the correct behavior ? distance is unstoppable but result is stoppable + pass_judge.pass_judged = false; + EXPECT_TRUE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_TRUE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); +} + +TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) +{ + using autoware::behavior_velocity_planner::no_stopping_area:: + generate_ego_no_stopping_area_lane_polygon; + using autoware::universe_utils::Point2d; + geometry_msgs::msg::Pose ego_pose; // ego at (0,0) + ego_pose.position.x = 0.0; + ego_pose.position.y = 0.0; + const tier4_planning_msgs::msg::PathWithLaneId path = + generate_straight_path(8); // ego path at x = 0, 1,..., 7 and y=0 + double margin = 1.0; // desired margin added to the polygon after the end of the area + double max_polygon_length = 10.0; // maximum length of the generated polygon + double path_expand_width = 2.0; // width of the generated polygon + auto logger = rclcpp::get_logger("test_logger"); + logger.set_level(rclcpp::Logger::Level::Debug); + rclcpp::Clock clock; + + lanelet::Polygon3d no_stopping_area; + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + const auto no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {no_stopping_area}, {}); + // basic case + { + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + // TODO(someone): we expect x=6 (end of area [5] + margin [1]) but current is 5.5 because a + // point is counted twice + // EXPECT_TRUE(point_in_polygon(Point2d(6.0, 2.0), simplified_poly)); + // EXPECT_TRUE(point_in_polygon(Point2d(6.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + + // big margin -> get a polygon until the end of the path + { + const double big_margin = 10.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, big_margin, max_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + EXPECT_TRUE(point_in_polygon(Point2d(7.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(7.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + // small max polygon length + { + const double small_polygon_length = 5.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, small_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + // TODO(someone): the length calculation is currently buggy + // ego at x=0, no_stopping_area starts at x=3 so we expect a lane polygon of length = 2.0, but + // some point is counted twice so the length is only 1.5 (interpolation interval is 0.5) + // EXPECT_TRUE(point_in_polygon(Point2d(4.0, 2.0), simplified_poly)); + // EXPECT_TRUE(point_in_polygon(Point2d(4.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + + // cases where the polygon returned is empty + // path is empty + { + tier4_planning_msgs::msg::PathWithLaneId empty_path; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + empty_path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // path points do not enter the no stopping area + // ego is far from the path + { + auto far_ego_pose = ego_pose; + far_ego_pose.position.y = 10.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, far_ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, + logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // no stopping area starts after the minimum length + { + const double short_max_polygon_length = 2.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, short_max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // path crosses the no stopping area but the current interpolation (0.5) is not enough to detect + // it + // TODO(someone): should this be fixed ? + { + lanelet::Polygon3d small_no_stopping_area; + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.1, -1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.1, 1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.4, 1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.4, -1.0)); + const auto small_no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {small_no_stopping_area}, {}); + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *small_no_stopping_area_reg_elem, margin, max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } +} + +TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) +{ + using autoware::behavior_velocity_planner::no_stopping_area::check_stop_lines_in_no_stopping_area; + tier4_planning_msgs::msg::PathWithLaneId path; + autoware::universe_utils::Polygon2d poly; + autoware::behavior_velocity_planner::no_stopping_area::DebugData debug_data; + + // empty inputs + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + + constexpr auto nb_points = 10; + constexpr auto non_stopped_velocity = 5.0; + const tier4_planning_msgs::msg::PathWithLaneId non_stopping_path = + generate_straight_path(nb_points, non_stopped_velocity); + path = non_stopping_path; + // set x=4 and x=5 to be stopping points + path.points[4].point.longitudinal_velocity_mps = 0.0; + path.points[5].point.longitudinal_velocity_mps = 0.0; + // empty polygon + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // non stopping path + poly.outer().emplace_back(3.0, -1.0); + poly.outer().emplace_back(3.0, 1.0); + poly.outer().emplace_back(5.0, 1.0); + poly.outer().emplace_back(5.0, -1.0); + poly.outer().push_back(poly.outer().front()); // close the polygon + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(non_stopping_path, poly, debug_data)); + // stop in the area + EXPECT_TRUE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // if stop in the area is within 1m of the end of the path we ignore it: only for 1st stop + path.points[8].point.longitudinal_velocity_mps = 0.0; + path.points[9].point.longitudinal_velocity_mps = 0.0; + EXPECT_TRUE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // if stop in the area is within 1m of the end of the path we ignore it + path.points[4].point.longitudinal_velocity_mps = non_stopped_velocity; + path.points[5].point.longitudinal_velocity_mps = non_stopped_velocity; + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); +} + +TEST(NoStoppingAreaTest, getStopLineGeometry2d) +{ + using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; + using autoware::behavior_velocity_planner::no_stopping_area::get_stop_line_geometry2d; + const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); + lanelet::Polygon3d no_stopping_area; + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + double stop_line_margin = 1.0; + double stop_line_extend_length = 1.0; + double vehicle_width = 1.0; + { // get stop line of the regulatory element extended by the extend length + lanelet::LineString3d reg_elem_stop_line; + reg_elem_stop_line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 0.0)); + reg_elem_stop_line.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 0.0)); + const auto no_stopping_area_reg_elem = lanelet::autoware::NoStoppingArea::make( + lanelet::InvalId, {}, {no_stopping_area}, reg_elem_stop_line); + const auto stop_line = get_stop_line_geometry2d( + path, *no_stopping_area_reg_elem, stop_line_margin, stop_line_extend_length, vehicle_width); + ASSERT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), -1.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 2.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + } + { // regulatory element has no stop line -> get the same stop line as generate_stop_line + const auto no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {no_stopping_area}, {}); + const auto stop_line = get_stop_line_geometry2d( + path, *no_stopping_area_reg_elem, stop_line_margin, stop_line_extend_length, vehicle_width); + const auto generated_stop_line = + generate_stop_line(path, {no_stopping_area}, vehicle_width, stop_line_margin); + ASSERT_TRUE(stop_line.has_value()); + ASSERT_TRUE(generated_stop_line.has_value()); + ASSERT_EQ(stop_line->size(), generated_stop_line->size()); + for (auto i = 0UL; i < stop_line->size(); ++i) { + EXPECT_EQ(stop_line->at(i).x(), generated_stop_line->at(i).x()); + EXPECT_EQ(stop_line->at(i).y(), generated_stop_line->at(i).y()); + } + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 29f3794516ef5..9dd645f08fdad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -274,7 +274,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { bool enable_rtc = true; From 8c63c61381b88527ffb5fb2ce8120db5e53f1179 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Tue, 15 Oct 2024 12:31:21 +0900 Subject: [PATCH 19/27] refactor(bpp_common, motion_utils): move path shifter util functions to autoware::motion_utils (#9081) * remove unused function Signed-off-by: Go Sakayori * mover path shifter utils function to autoware motion utils Signed-off-by: Go Sakayori * minor change in license header Signed-off-by: Go Sakayori * fix warning message Signed-off-by: Go Sakayori * remove header file Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../motion_utils/trajectory/path_shift.hpp | 71 ++++++++++++ .../src/trajectory/path_shift.cpp | 105 ++++++++++++++++++ .../src/pull_over_planner/shift_pull_over.cpp | 3 +- .../src/scene.cpp | 9 +- .../src/utils/calculation.cpp | 10 +- .../utils/path_shifter/path_shifter.hpp | 31 ------ .../src/utils/path_shifter/path_shifter.cpp | 101 ----------------- .../src/scene.cpp | 3 +- .../src/shift_pull_out.cpp | 3 +- .../helper.hpp | 17 +-- .../src/scene.cpp | 10 +- .../src/shift_line_generator.cpp | 4 +- 12 files changed, 210 insertions(+), 157 deletions(-) create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp create mode 100644 common/autoware_motion_utils/src/trajectory/path_shift.cpp diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp new file mode 100644 index 0000000000000..f4602ffff83e4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp @@ -0,0 +1,71 @@ +// 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 AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ + +namespace autoware::motion_utils +{ +/** + * @brief Calculates the velocity required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param longitudinal_distance longitudinal distance + * @return velocity + */ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance); + +/** + * @brief Calculates the lateral distance required for shifting + * @param longitudinal longitudinal distance + * @param jerk lateral jerk + * @param velocity velocity + * @return lateral distance + */ +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity); + +/** + * @brief Calculates the lateral distance required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param velocity velocity + * @return longitudinal distance + */ +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity); + +/** + * @brief Calculates the total time required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param acc lateral acceleration + * @return time + */ +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc); + +/** + * @brief Calculates the required jerk from lateral/longitudinal distance + * @param lateral lateral distance + * @param longitudinal longitudinal distance + * @param velocity velocity + * @return jerk + */ +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory/path_shift.cpp b/common/autoware_motion_utils/src/trajectory/path_shift.cpp new file mode 100644 index 0000000000000..6f57f34a56190 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/path_shift.cpp @@ -0,0 +1,105 @@ +// 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 "autoware/motion_utils/trajectory/path_shift.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +namespace autoware::motion_utils +{ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double d = std::abs(longitudinal_distance); + if (j < 1.0e-8 || l < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate velocity due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); +} + +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double d = std::abs(longitudinal); + const double v = std::abs(velocity); + + if (j < 1.0e-8 || v < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate lateral distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; +} + +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate longitudinal distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; +} + +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc) +{ + const double j = std::abs(jerk); + const double a = std::abs(acc); + const double l = std::abs(lateral); + if (j < 1.0e-8 || a < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate shift time due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + + // time with constant jerk + double tj = a / j; + + // time with constant acceleration (zero jerk) + double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); + + if (ta < 0.0) { + // it will not hit the acceleration limit this time + tj = std::pow(l / (2.0 * j), 1.0 / 3.0); + ta = 0.0; + } + + const double t_total = 4.0 * tj + 2.0 * ta; + return t_total; +} + +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity) +{ + constexpr double ep = 1.0e-3; + const double lat = std::abs(lateral); + const double lon = std::max(std::abs(longitudinal), ep); + const double v = std::abs(velocity); + return 0.5 * lat * std::pow(4.0 * v / lon, 3); +} + +} // namespace autoware::motion_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 30f250634c028..f70dbbd9c1e50 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include @@ -181,7 +182,7 @@ std::optional ShiftPullOver::generatePullOverPath( .y; // calculate shift start pose on road lane - const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( + const double pull_over_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 532a99a67195b..5961435085346 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -23,6 +23,8 @@ #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include #include #include #include @@ -1612,7 +1614,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto [min_lateral_acc, max_lateral_acc] = lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; @@ -1876,9 +1878,8 @@ bool NormalLaneChange::calcAbortPath() PathShifter path_shifter; path_shifter.setPath(lane_changing_path); path_shifter.addShiftLine(shift_line); - const auto lateral_jerk = - autoware::behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( - shift_line.end_shift_length, abort_start_dist, current_velocity); + const auto lateral_jerk = autoware::motion_utils::calc_jerk_from_lat_lon_distance( + shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); const auto lateral_acc_range = lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 1c4aede51074a..762cada4e59d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -15,6 +15,7 @@ #include #include +#include #include @@ -163,7 +164,7 @@ double calc_maximum_lane_change_length( const auto [min_lat_acc, max_lat_acc] = lane_change_parameters.lane_change_lat_acc_map.find(vel); const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; @@ -208,7 +209,8 @@ std::vector calc_all_min_lc_lengths( min_lc_lengths.reserve(shift_intervals.size()); const auto min_lc_length = [&](const auto shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto t = + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); return min_vel * t; }; @@ -248,7 +250,7 @@ std::vector calc_all_max_lc_lengths( // lane changing section const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; @@ -410,7 +412,7 @@ std::vector calc_shift_phase_metrics( for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; lat_acc += lateral_acc_resolution) { - const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + const auto lane_changing_duration = autoware::motion_utils::calc_shift_time_from_jerk( shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 8164ef659b7b1..4364bb1744a7e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -108,11 +108,6 @@ class PathShifter */ std::vector getShiftLines() const { return shift_lines_; } - /** - * @brief Get shift points size. - */ - size_t getShiftLinesSize() const { return shift_lines_.size(); } - /** * @brief Get base offset. */ @@ -138,36 +133,10 @@ class PathShifter */ void removeBehindShiftLineAndSetBaseOffset(const size_t nearest_idx); - //////////////////////////////////////// - // Utility Functions - //////////////////////////////////////// - - static double calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double longitudinal_distance); - - static double calcLateralDistFromJerk( - const double longitudinal, const double jerk, const double velocity); - - static double calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity); - - static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc); - - static double calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity); - - double getTotalShiftLength() const; - double getLastShiftLength() const; std::optional getLastShiftLine() const; - /** - * @brief Calculate the theoretical lateral jerk by spline shifting for current shift_lines_. - * @return Jerk array. The size is same as the shift points. - */ - std::vector calcLateralJerk() const; - private: // The reference path along which the shift will be performed. PathWithLaneId reference_path_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 69c538fa6cb54..22baffe80fdf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -395,27 +395,6 @@ std::pair, std::vector> PathShifter::calcBaseLengths return {base_lon, base_lat}; } -std::vector PathShifter::calcLateralJerk() const -{ - const auto arclength_arr = utils::calcPathArcLengthArray(reference_path_); - - constexpr double epsilon = 1.0e-8; // to avoid 0 division - - std::vector lateral_jerk{}; - - // TODO(Watanabe) write docs. - double current_shift = base_offset_; - for (const auto & shift_line : shift_lines_) { - const double delta_shift = shift_line.end_shift_length - current_shift; - const auto shifting_arclength = std::max( - arclength_arr.at(shift_line.end_idx) - arclength_arr.at(shift_line.start_idx), epsilon); - lateral_jerk.push_back((delta_shift / 2.0) / std::pow(shifting_arclength / 4.0, 3.0)); - current_shift = shift_line.end_shift_length; - } - - return lateral_jerk; -} - void PathShifter::updateShiftLinesIndices(ShiftLineArray & shift_lines) const { if (reference_path_.points.empty()) { @@ -542,86 +521,6 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) } } -double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc) -{ - const double j = std::abs(jerk); - const double a = std::abs(acc); - const double l = std::abs(lateral); - if (j < 1.0e-8 || a < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - - // time with constant jerk - double tj = a / j; - - // time with constant acceleration (zero jerk) - double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); - - if (ta < 0.0) { - // it will not hit the acceleration limit this time - tj = std::pow(l / (2.0 * j), 1.0 / 3.0); - ta = 0.0; - } - - const double t_total = 4.0 * tj + 2.0 * ta; - return t_total; -} - -double PathShifter::calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double longitudinal_distance) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double d = std::abs(longitudinal_distance); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); -} - -double PathShifter::calcLateralDistFromJerk( - const double longitudinal, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double d = std::abs(longitudinal); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; -} - -double PathShifter::calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; -} - -double PathShifter::calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity) -{ - constexpr double ep = 1.0e-3; - const double lat = std::abs(lateral); - const double lon = std::max(std::abs(longitudinal), ep); - const double v = std::abs(velocity); - return 0.5 * lat * std::pow(4.0 * v / lon, 3); -} - -double PathShifter::getTotalShiftLength() const -{ - double sum = base_offset_; - for (const auto & l : shift_lines_) { - sum += l.end_shift_length; - } - return sum; -} - double PathShifter::getLastShiftLength() const { if (shift_lines_.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index d831f65b23117..e1f6d0bb6af9c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -20,6 +20,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_side_shift_module/utils.hpp" +#include #include #include @@ -346,7 +347,7 @@ ShiftLine SideShiftModule::calcShiftLine() const const double dist_to_end = [&]() { const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); - const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( + const double jerk_shifting_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance); const double dist_to_end = dist_to_start + shifting_distance; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 3f750a7f1a87b..2d94ceb85de28 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -22,6 +22,7 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include #include #include @@ -306,7 +307,7 @@ std::vector ShiftPullOut::calcPullOutPaths( path_shifter.setPath(road_lane_reference_path); const double shift_time = - PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_length, lateral_jerk, lateral_acc); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0); const auto pull_out_distance = calcPullOutLongitudinalDistance( longitudinal_acc, shift_time, shift_length, maximum_curvature, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 0f3536eeb4e7c..b59dc9e91e23c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -20,6 +20,8 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include + #include #include #include @@ -118,26 +120,27 @@ class AvoidanceHelper const auto nominal_speed = std::max(getEgoSpeed(), p->nominal_avoidance_speed); const auto nominal_jerk = p->lateral_min_jerk_map.at(getConstraintsMapIndex(nominal_speed, p->velocity_map)); - return PathShifter::calcLongitudinalDistFromJerk(shift_length, nominal_jerk, nominal_speed); + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( + shift_length, nominal_jerk, nominal_speed); } double getMinAvoidanceDistance(const double shift_length) const { const auto & p = parameters_; - return PathShifter::calcLongitudinalDistFromJerk( + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->lateral_max_jerk_map.front(), p->velocity_map.front()); } double getMaxAvoidanceDistance(const double shift_length) const { - const auto distance_from_jerk = PathShifter::calcLongitudinalDistFromJerk( + const auto distance_from_jerk = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, getLateralMinJerkLimit(), getAvoidanceEgoSpeed()); return std::max(getNominalAvoidanceDistance(shift_length), distance_from_jerk); } double getSharpAvoidanceDistance(const double shift_length) const { - return PathShifter::calcLongitudinalDistFromJerk( + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } @@ -227,8 +230,8 @@ class AvoidanceHelper const auto max_shift_length = std::max( std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); - const auto dynamic_distance = - PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed); + const auto dynamic_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( + max_shift_length, getLateralMinJerkLimit(), speed); return std::clamp( 1.5 * dynamic_distance + getNominalPrepareDistance(), @@ -312,7 +315,7 @@ class AvoidanceHelper { const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { - return PathShifter::calcJerkFromLatLonDistance( + return autoware::motion_utils::calc_jerk_from_lat_lon_distance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < getLateralMaxJerkLimit() + JERK_BUFFER; }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 4d13d4164d263..2c5950bfeae0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1316,7 +1316,7 @@ void StaticObstacleAvoidanceModule::addNewShiftLines( const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) .point.longitudinal_velocity_mps; - const double shift_time = PathShifter::calcShiftTimeFromJerk( + const double shift_time = autoware::motion_utils::calc_shift_time_from_jerk( front_new_shift_line.getRelativeLength(), helper_->getLateralMaxJerkLimit(), helper_->getLateralMaxAccelLimit()); const double longitudinal_acc = @@ -1673,7 +1673,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); // insert slow down speed. - const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + const double current_target_velocity = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), to_stop_line); if (current_target_velocity < getEgoSpeed()) { RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); @@ -1692,7 +1692,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( } // target speed with nominal jerk limits. - const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + const double v_target = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = @@ -1880,7 +1880,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. - const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + const double current_target_velocity = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::static_obstacle_avoidance::insertDecelPoint( @@ -1901,7 +1901,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ } // target speed with nominal jerk limits. - const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + const double v_target = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 10d003fcad90f..917736b1680db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -179,7 +179,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // calculate lateral jerk. - const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( + const auto required_jerk = autoware::motion_utils::calc_jerk_from_lat_lon_distance( avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. @@ -201,7 +201,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // output avoidance path under lateral jerk constraints. - const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = autoware::motion_utils::calc_lateral_dist_from_jerk( avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { From bb825f0021e5aa0e1962ca982bb537b1562df2da Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Tue, 15 Oct 2024 14:12:45 +0900 Subject: [PATCH 20/27] test(motion_utils): add test for path shift (#9083) * remove unused function Signed-off-by: Go Sakayori * mover path shifter utils function to autoware motion utils Signed-off-by: Go Sakayori * minor change in license header Signed-off-by: Go Sakayori * fix warning message Signed-off-by: Go Sakayori * remove header file Signed-off-by: Go Sakayori * add test file Signed-off-by: Go Sakayori * add unit test to all function Signed-off-by: Go Sakayori * fix spelling Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../test/src/trajectory/test_path_shift.cpp | 163 ++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp new file mode 100644 index 0000000000000..fbdcc7a8f6f81 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp @@ -0,0 +1,163 @@ +// 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 "autoware/motion_utils/trajectory/path_shift.hpp" + +#include + +TEST(path_shift_test, calc_feasible_velocity_from_jerk) +{ + using autoware::motion_utils::calc_feasible_velocity_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = 1.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero longitudinal distance + lateral_distance = 2.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0); + + // Condition: random condition + longitudinal_distance = 50.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5); +} + +TEST(path_shift_test, calc_lateral_dist_from_jerk) +{ + using autoware::motion_utils::calc_lateral_dist_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero velocity + lateral_jerk = 2.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero longitudinal distance + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0); + + // Condition: random condition + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5); +} + +TEST(path_shift_test, calc_longitudinal_dist_from_jerk) +{ + using autoware::motion_utils::calc_longitudinal_dist_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = -1.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: zero velocity + velocity = 0.0; + lateral_distance = 54.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: random + velocity = 8.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0); +} + +TEST(path_shift_test, calc_shift_time_from_jerk) +{ + constexpr double epsilon = 1e-6; + + using autoware::motion_utils::calc_shift_time_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double lateral_acceleration = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral acceleration + lateral_jerk = -2.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral distance + lateral_acceleration = -4.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0); + + // Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough) + lateral_distance = 80.0; + EXPECT_NEAR( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139, + epsilon); +} + +TEST(path_shift_test, calc_jerk_from_lat_lon_distance) +{ + using autoware::motion_utils::calc_jerk_from_lat_lon_distance; + + double lateral_distance = 0.0; + double longitudinal_distance = 100.0; + double velocity = 10.0; + + // Condition: zero lateral distance + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero velocity + lateral_distance = 5.0; + velocity = 0.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero longitudinal distance + longitudinal_distance = 0.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14); + + // Condition: random + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16); +} From 328ffa9fb4c6ab0084ffcc65f8a428ed32252431 Mon Sep 17 00:00:00 2001 From: Kazunori-Nakajima <169115284+Kazunori-Nakajima@users.noreply.github.com> Date: Tue, 15 Oct 2024 17:54:28 +0900 Subject: [PATCH 21/27] feat(costmap_generator, control_validator, scenario_selector, surround_obstacle_checker, vehicle_cmd_gate): add processing time pub. (#9065) * feat(costmap_generator, control_validator, scenario_selector, surround_obstacle_checker, vehicle_cmd_gate): Add: processing_time_pub Signed-off-by: Kazunori-Nakajima * fix: pre-commit Signed-off-by: Kasunori-Nakajima * feat(costmap_generator): fix: No output when not Active. Signed-off-by: Kasunori-Nakajima * fix: clang-format Signed-off-by: Kasunori-Nakajima * Re: fix: clang-format Signed-off-by: Kasunori-Nakajima --------- Signed-off-by: Kazunori-Nakajima Signed-off-by: Kasunori-Nakajima --- .../control_validator/control_validator.hpp | 5 +++++ .../src/control_validator.cpp | 11 ++++++++++ .../src/vehicle_cmd_gate.cpp | 22 +++++++++++++++++++ .../src/vehicle_cmd_gate.hpp | 6 +++++ .../costmap_generator.hpp | 7 ++++++ .../costmap_generator_node.cpp | 14 ++++++++++++ .../autoware/scenario_selector/node.hpp | 6 +++++ .../autoware_scenario_selector/src/node.cpp | 11 ++++++++++ .../src/node.cpp | 11 ++++++++++ .../src/node.hpp | 2 ++ .../config/processing_time_checker.param.yaml | 5 +++++ 11 files changed, 100 insertions(+) diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 56a1e39391b10..551ee08aa3327 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -20,12 +20,14 @@ #include "autoware_vehicle_info_utils/vehicle_info.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" +#include #include #include #include #include #include +#include #include #include @@ -143,6 +145,7 @@ class ControlValidator : public rclcpp::Node universe_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // system parameters int64_t diag_error_count_threshold_ = 0; @@ -167,6 +170,8 @@ class ControlValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; + autoware::universe_utils::StopWatch stop_watch; + std::shared_ptr debug_pose_publisher_; }; } // namespace autoware::control_validator diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 2370d274fecc8..d601319d47bc5 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -47,6 +47,9 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) pub_markers_ = create_publisher("~/output/markers", 1); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); + debug_pose_publisher_ = std::make_shared(this); setup_parameters(); @@ -134,6 +137,8 @@ bool ControlValidator::is_data_ready() void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { + stop_watch.tic(); + current_predicted_trajectory_ = msg; current_reference_trajectory_ = sub_reference_traj_->takeData(); current_kinematics_ = sub_kinematics_->takeData(); @@ -162,6 +167,12 @@ void ControlValidator::publish_debug_info() debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); } debug_pose_publisher_->publish(); + + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); } void ControlValidator::validate( diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index bf61b69857849..b028455ab3354 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -77,6 +77,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_pub_ = create_publisher("output/engage", durable_qos); pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); + processing_time_pub_ = + this->create_publisher("~/debug/processing_time_ms", 1); is_filter_activated_pub_ = create_publisher("~/is_filter_activated", durable_qos); @@ -375,6 +377,8 @@ T VehicleCmdGate::getContinuousTopic( void VehicleCmdGate::onTimer() { + stop_watch.tic(); + // Subscriber for auto const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData(); if (msg_auto_command_turn_indicator) @@ -573,12 +577,18 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_); vehicle_cmd_emergency.stamp = filtered_control.stamp; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + // Publish commands vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_control); published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp); adapi_pause_->publish(); moderate_stop_interface_->publish(); + processing_time_pub_->publish(processing_time_msg); // Save ControlCmd to steering angle when disengaged prev_control_cmd_ = filtered_control; @@ -616,12 +626,18 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() vehicle_cmd_emergency.stamp = stamp; vehicle_cmd_emergency.emergency = true; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + // Publish topics vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(control_cmd); turn_indicator_cmd_pub_->publish(turn_indicator); hazard_light_cmd_pub_->publish(hazard_light); gear_cmd_pub_->publish(gear); + processing_time_pub_->publish(processing_time_msg); } void VehicleCmdGate::publishStatus() @@ -638,12 +654,18 @@ void VehicleCmdGate::publishStatus() external_emergency.stamp = stamp; external_emergency.emergency = is_external_emergency_stop_; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + gate_mode_pub_->publish(current_gate_mode_); engage_pub_->publish(autoware_engage); pub_external_emergency_->publish(external_emergency); operation_mode_pub_->publish(current_operation_mode_); adapi_pause_->publish(); moderate_stop_interface_->publish(); + processing_time_pub_->publish(processing_time_msg); } Control VehicleCmdGate::filterControlCommand(const Control & in) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 6863015443363..c2716ea3baa6a 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +116,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; + rclcpp::Publisher::SharedPtr processing_time_pub_; // Parameter callback OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( @@ -268,6 +271,9 @@ class VehicleCmdGate : public rclcpp::Node std::unique_ptr vehicle_stop_checker_; double stop_check_duration_; + // processing time + autoware::universe_utils::StopWatch stop_watch; + // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp index b08d16c68fa6f..434440359f282 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp @@ -49,6 +49,8 @@ #include "autoware_costmap_generator/points_to_costmap.hpp" #include "costmap_generator_node_parameters.hpp" +#include +#include #include #include #include @@ -58,6 +60,7 @@ #include #include #include +#include #include #include @@ -91,6 +94,7 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_costmap_; rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_points_; rclcpp::Subscription::SharedPtr sub_objects_; @@ -175,6 +179,9 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); + + /// \brief measure processing time + autoware::universe_utils::StopWatch stop_watch; }; } // namespace autoware::costmap_generator diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 9012b7f3250b4..07bf9eb33ffbe 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -192,6 +192,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) pub_processing_time_ = create_publisher("processing_time", 1); time_keeper_ = std::make_shared(pub_processing_time_); + pub_processing_time_ms_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -277,7 +279,13 @@ void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::Cons void CostmapGenerator::onTimer() { autoware::universe_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_); + stop_watch.tic(); if (!isActive()) { + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_ms_->publish(processing_time_msg); return; } @@ -467,6 +475,12 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap); out_gridmap_msg->header = header; pub_costmap_->publish(*out_gridmap_msg); + + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_ms_->publish(processing_time_msg); } } // namespace autoware::costmap_generator diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 487c643fe5673..244876ee2cb22 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -37,6 +38,7 @@ #endif #include #include +#include #include #include @@ -95,6 +97,7 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // polling subscribers universe_utils::InterProcessPollingSubscriber< @@ -130,6 +133,9 @@ class ScenarioSelectorNode : public rclcpp::Node static constexpr double lane_stopping_timeout_s = 5.0; static constexpr double empty_parking_trajectory_timeout_s = 3.0; + + // processing time + autoware::universe_utils::StopWatch stop_watch; }; } // namespace autoware::scenario_selector #endif // AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 1f4547d04c8fc..eb3303766205f 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -330,6 +330,9 @@ bool ScenarioSelectorNode::isDataReady() void ScenarioSelectorNode::updateData() { + { + stop_watch.tic(); + } { auto msg = sub_parking_state_->takeData(); is_parking_completed_ = msg ? msg->data : is_parking_completed_; @@ -370,6 +373,12 @@ void ScenarioSelectorNode::onTimer() } pub_scenario_->publish(scenario); + + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); } void ScenarioSelectorNode::onLaneDrivingTrajectory( @@ -466,6 +475,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = std::make_unique(this); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 719a507ab569b..da0dd0ec09a26 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -109,6 +110,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -151,6 +154,9 @@ bool SurroundObstacleCheckerNode::getUseDynamicObject() const void SurroundObstacleCheckerNode::onTimer() { + autoware::universe_utils::StopWatch stop_watch; + stop_watch.tic(); + odometry_ptr_ = sub_odometry_.takeData(); pointcloud_ptr_ = sub_pointcloud_.takeData(); object_ptr_ = sub_dynamic_objects_.takeData(); @@ -256,6 +262,11 @@ void SurroundObstacleCheckerNode::onTimer() no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); } + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); + pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f04a3aba07baf..480a937a4a909 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -106,6 +107,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml index 84e7d79079dae..033b20d234fd9 100644 --- a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml +++ b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml @@ -2,9 +2,11 @@ ros__parameters: update_rate: 10.0 processing_time_topic_name_list: + - /control/control_validator/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms - /control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms + - /control/vehicle_cmd_gate/debug/processing_time_ms - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms - /planning/planning_validator/debug/processing_time_ms @@ -33,5 +35,8 @@ - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/processing_time_ms + - /planning/scenario_planning/parking/costmap_generator/debug/processing_time_ms + - /planning/scenario_planning/scenario_selector/debug/processing_time_ms - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms - /simulation/shape_estimation/debug/processing_time_ms From 08d4914d04282c5cb059d34163ed09b02d0b5a46 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 15 Oct 2024 11:02:45 +0000 Subject: [PATCH 22/27] refactor(polar_grid): add autoware prefix to polar_grid (#8945) Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 6 +++--- .../{polar_grid => autoware_polar_grid}/Readme.md | 0 .../icons/classes/PolarGridDisplay.png | Bin .../{polar_grid => autoware_polar_grid}/package.xml | 4 ++-- .../plugins/plugin_description.xml | 2 +- .../src/polar_grid_display.cpp | 0 .../src/polar_grid_display.hpp | 0 7 files changed, 6 insertions(+), 6 deletions(-) rename common/{polar_grid => autoware_polar_grid}/CMakeLists.txt (85%) rename common/{polar_grid => autoware_polar_grid}/Readme.md (100%) rename common/{polar_grid => autoware_polar_grid}/icons/classes/PolarGridDisplay.png (100%) rename common/{polar_grid => autoware_polar_grid}/package.xml (90%) rename common/{polar_grid => autoware_polar_grid}/plugins/plugin_description.xml (81%) rename common/{polar_grid => autoware_polar_grid}/src/polar_grid_display.cpp (100%) rename common/{polar_grid => autoware_polar_grid}/src/polar_grid_display.hpp (100%) diff --git a/common/polar_grid/CMakeLists.txt b/common/autoware_polar_grid/CMakeLists.txt similarity index 85% rename from common/polar_grid/CMakeLists.txt rename to common/autoware_polar_grid/CMakeLists.txt index 9932dc594d552..efa068a3f64a4 100644 --- a/common/polar_grid/CMakeLists.txt +++ b/common/autoware_polar_grid/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(polar_grid) +project(autoware_polar_grid) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,12 +10,12 @@ set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) -ament_auto_add_library(polar_grid SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/polar_grid_display.cpp src/polar_grid_display.hpp ) -target_link_libraries(polar_grid +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) diff --git a/common/polar_grid/Readme.md b/common/autoware_polar_grid/Readme.md similarity index 100% rename from common/polar_grid/Readme.md rename to common/autoware_polar_grid/Readme.md diff --git a/common/polar_grid/icons/classes/PolarGridDisplay.png b/common/autoware_polar_grid/icons/classes/PolarGridDisplay.png similarity index 100% rename from common/polar_grid/icons/classes/PolarGridDisplay.png rename to common/autoware_polar_grid/icons/classes/PolarGridDisplay.png diff --git a/common/polar_grid/package.xml b/common/autoware_polar_grid/package.xml similarity index 90% rename from common/polar_grid/package.xml rename to common/autoware_polar_grid/package.xml index ece156669c85b..3b376b3a75185 100644 --- a/common/polar_grid/package.xml +++ b/common/autoware_polar_grid/package.xml @@ -1,9 +1,9 @@ - polar_grid + autoware_polar_grid 0.1.0 - The polar_grid package + The autoware_polar_grid package Yukihiro Saito Apache License 2.0 diff --git a/common/polar_grid/plugins/plugin_description.xml b/common/autoware_polar_grid/plugins/plugin_description.xml similarity index 81% rename from common/polar_grid/plugins/plugin_description.xml rename to common/autoware_polar_grid/plugins/plugin_description.xml index 1ccaf8725d4de..a09fb5ed6a5d4 100644 --- a/common/polar_grid/plugins/plugin_description.xml +++ b/common/autoware_polar_grid/plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/common/polar_grid/src/polar_grid_display.cpp b/common/autoware_polar_grid/src/polar_grid_display.cpp similarity index 100% rename from common/polar_grid/src/polar_grid_display.cpp rename to common/autoware_polar_grid/src/polar_grid_display.cpp diff --git a/common/polar_grid/src/polar_grid_display.hpp b/common/autoware_polar_grid/src/polar_grid_display.hpp similarity index 100% rename from common/polar_grid/src/polar_grid_display.hpp rename to common/autoware_polar_grid/src/polar_grid_display.hpp From b50235190f945cf09111d7f2a8f9c9cc1bc91f27 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Tue, 15 Oct 2024 20:57:33 +0900 Subject: [PATCH 23/27] refactor(freespace_planner): move functions to utils (#9058) * refactor freespace planner Signed-off-by: mohammad alqudah * add function is_near_target to freespace planner utils Signed-off-by: mohammad alqudah * add freespace planner utils namespace Signed-off-by: mohammad alqudah * fix function call Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../autoware_freespace_planner/CMakeLists.txt | 1 + .../autoware/freespace_planner/utils.hpp | 78 +++++ .../freespace_planner_node.cpp | 268 ++++-------------- .../src/autoware_freespace_planner/utils.cpp | 178 ++++++++++++ 4 files changed, 306 insertions(+), 219 deletions(-) create mode 100644 planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp create mode 100644 planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp diff --git a/planning/autoware_freespace_planner/CMakeLists.txt b/planning/autoware_freespace_planner/CMakeLists.txt index 7901be70fbc63..bbbb09c7d5af1 100644 --- a/planning/autoware_freespace_planner/CMakeLists.txt +++ b/planning/autoware_freespace_planner/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(freespace_planner_node SHARED src/autoware_freespace_planner/freespace_planner_node.cpp + src/autoware_freespace_planner/utils.cpp ) rclcpp_components_register_node(freespace_planner_node diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp new file mode 100644 index 0000000000000..4a7f3b1cd6c2b --- /dev/null +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp @@ -0,0 +1,78 @@ +// 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 AUTOWARE__FREESPACE_PLANNER__UTILS_HPP_ +#define AUTOWARE__FREESPACE_PLANNER__UTILS_HPP_ + +#include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::freespace_planner::utils +{ +using autoware::freespace_planning_algorithms::PlannerWaypoint; +using autoware::freespace_planning_algorithms::PlannerWaypoints; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseArray; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::Odometry; +using tier4_planning_msgs::msg::Scenario; + +PoseArray trajectory_to_pose_array(const Trajectory & trajectory); + +double calc_distance_2d(const Trajectory & trajectory, const Pose & pose); + +Pose transform_pose(const Pose & pose, const TransformStamped & transform); + +bool is_active(const Scenario::ConstSharedPtr & scenario); + +std::vector get_reversing_indices(const Trajectory & trajectory); + +size_t get_next_target_index( + const size_t trajectory_size, const std::vector & reversing_indices, + const size_t current_target_index); + +Trajectory get_partial_trajectory( + const Trajectory & trajectory, const size_t start_index, const size_t end_index); + +Trajectory create_trajectory( + const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, + const double & velocity); + +Trajectory create_stop_trajectory(const PoseStamped & current_pose); + +Trajectory create_stop_trajectory(const Trajectory & trajectory); + +bool is_stopped( + const std::deque & odom_buffer, const double th_stopped_velocity_mps); + +bool is_near_target( + const Pose & target_pose, const Pose & current_pose, const double th_distance_m); +} // namespace autoware::freespace_planner::utils + +#endif // AUTOWARE__FREESPACE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 267fd9932e781..a2a4d1f079b3c 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -30,6 +30,7 @@ #include "autoware/freespace_planner/freespace_planner_node.hpp" +#include "autoware/freespace_planner/utils.hpp" #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" #include @@ -43,181 +44,6 @@ #include #include -namespace -{ -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; -using autoware::freespace_planning_algorithms::AstarSearch; -using autoware::freespace_planning_algorithms::PlannerWaypoint; -using autoware::freespace_planning_algorithms::PlannerWaypoints; -using autoware::freespace_planning_algorithms::RRTStar; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::PoseStamped; -using geometry_msgs::msg::TransformStamped; -using geometry_msgs::msg::Twist; -using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::Scenario; - -bool isActive(const Scenario::ConstSharedPtr & scenario) -{ - if (!scenario) { - return false; - } - - const auto & s = scenario->activating_scenarios; - if (std::find(std::begin(s), std::end(s), Scenario::PARKING) != std::end(s)) { - return true; - } - - return false; -} - -PoseArray trajectory2PoseArray(const Trajectory & trajectory) -{ - PoseArray pose_array; - pose_array.header = trajectory.header; - - for (const auto & point : trajectory.points) { - pose_array.poses.push_back(point.pose); - } - - return pose_array; -} - -std::vector getReversingIndices(const Trajectory & trajectory) -{ - std::vector indices; - - for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { - if ( - trajectory.points.at(i).longitudinal_velocity_mps * - trajectory.points.at(i + 1).longitudinal_velocity_mps < - 0) { - indices.push_back(i); - } - } - - return indices; -} - -size_t getNextTargetIndex( - const size_t trajectory_size, const std::vector & reversing_indices, - const size_t current_target_index) -{ - if (!reversing_indices.empty()) { - for (const auto reversing_index : reversing_indices) { - if (reversing_index > current_target_index) { - return reversing_index; - } - } - } - - return trajectory_size - 1; -} - -Trajectory getPartialTrajectory( - const Trajectory & trajectory, const size_t start_index, const size_t end_index) -{ - Trajectory partial_trajectory; - partial_trajectory.header = trajectory.header; - partial_trajectory.header.stamp = rclcpp::Clock().now(); - - partial_trajectory.points.reserve(trajectory.points.size()); - for (size_t i = start_index; i <= end_index; ++i) { - partial_trajectory.points.push_back(trajectory.points.at(i)); - } - - // Modify velocity at start/end point - if (partial_trajectory.points.size() >= 2) { - partial_trajectory.points.front().longitudinal_velocity_mps = - partial_trajectory.points.at(1).longitudinal_velocity_mps; - } - if (!partial_trajectory.points.empty()) { - partial_trajectory.points.back().longitudinal_velocity_mps = 0; - } - - return partial_trajectory; -} - -double calcDistance2d(const Trajectory & trajectory, const Pose & pose) -{ - const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); - return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); -} - -Pose transformPose(const Pose & pose, const TransformStamped & transform) -{ - PoseStamped transformed_pose; - PoseStamped orig_pose; - orig_pose.pose = pose; - tf2::doTransform(orig_pose, transformed_pose, transform); - - return transformed_pose.pose; -} - -Trajectory createTrajectory( - const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, - const double & velocity) -{ - Trajectory trajectory; - trajectory.header = planner_waypoints.header; - - for (const auto & awp : planner_waypoints.waypoints) { - TrajectoryPoint point; - - point.pose = awp.pose.pose; - - point.pose.position.z = current_pose.pose.position.z; // height = const - point.longitudinal_velocity_mps = velocity / 3.6; // velocity = const - - // switch sign by forward/backward - point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps; - - trajectory.points.push_back(point); - } - - return trajectory; -} - -Trajectory createStopTrajectory(const PoseStamped & current_pose) -{ - PlannerWaypoints waypoints; - PlannerWaypoint waypoint; - - waypoints.header.stamp = rclcpp::Clock().now(); - waypoints.header.frame_id = current_pose.header.frame_id; - waypoint.pose.header = waypoints.header; - waypoint.pose.pose = current_pose.pose; - waypoint.is_back = false; - waypoints.waypoints.push_back(waypoint); - - return createTrajectory(current_pose, waypoints, 0.0); -} - -Trajectory createStopTrajectory(const Trajectory & trajectory) -{ - Trajectory stop_trajectory = trajectory; - for (size_t i = 0; i < trajectory.points.size(); ++i) { - stop_trajectory.points.at(i).longitudinal_velocity_mps = 0.0; - } - return stop_trajectory; -} - -bool isStopped( - const std::deque & odom_buffer, const double th_stopped_velocity_mps) -{ - for (const auto & odom : odom_buffer) { - if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { - return false; - } - } - return true; -} - -} // namespace - namespace autoware::freespace_planner { FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_options) @@ -321,8 +147,8 @@ bool FreespacePlannerNode::isPlanRequired() } if (node_param_.replan_when_course_out) { - const bool is_course_out = - calcDistance2d(trajectory_, current_pose_.pose) > node_param_.th_course_out_distance_m; + const bool is_course_out = utils::calc_distance_2d(trajectory_, current_pose_.pose) > + node_param_.th_course_out_distance_m; if (is_course_out) { RCLCPP_INFO(get_logger(), "Course out"); return true; @@ -340,10 +166,10 @@ bool FreespacePlannerNode::checkCurrentTrajectoryCollision() partial_trajectory_.points, current_pose_.pose.position); const size_t end_index_partial = partial_trajectory_.points.size() - 1; const auto forward_trajectory = - getPartialTrajectory(partial_trajectory_, nearest_index_partial, end_index_partial); + utils::get_partial_trajectory(partial_trajectory_, nearest_index_partial, end_index_partial); const bool is_obs_found = - algo_->hasObstacleOnTrajectory(trajectory2PoseArray(forward_trajectory)); + algo_->hasObstacleOnTrajectory(utils::trajectory_to_pose_array(forward_trajectory)); if (!is_obs_found) { obs_found_time_ = {}; @@ -357,28 +183,30 @@ bool FreespacePlannerNode::checkCurrentTrajectoryCollision() void FreespacePlannerNode::updateTargetIndex() { - const double long_disp_to_target = autoware::universe_utils::calcLongitudinalDeviation( - trajectory_.points.at(target_index_).pose, current_pose_.pose.position); - const auto is_near_target = std::abs(long_disp_to_target) < node_param_.th_arrived_distance_m; - - const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); - - if (is_near_target && is_stopped) { - const auto new_target_index = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); - - if (new_target_index == target_index_) { - // Finished publishing all partial trajectories - is_completed_ = true; - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Freespace planning completed"); - std_msgs::msg::Bool is_completed_msg; - is_completed_msg.data = is_completed_; - parking_state_pub_->publish(is_completed_msg); - } else { - // Switch to next partial trajectory - prev_target_index_ = target_index_; - target_index_ = new_target_index; - } + if (!utils::is_stopped(odom_buffer_, node_param_.th_stopped_velocity_mps)) { + return; + } + + const auto is_near_target = utils::is_near_target( + trajectory_.points.at(target_index_).pose, current_pose_.pose, + node_param_.th_arrived_distance_m); + + if (!is_near_target) return; + + const auto new_target_index = + utils::get_next_target_index(trajectory_.points.size(), reversing_indices_, target_index_); + + if (new_target_index == target_index_) { + // Finished publishing all partial trajectories + is_completed_ = true; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Freespace planning completed"); + std_msgs::msg::Bool is_completed_msg; + is_completed_msg.data = is_completed_; + parking_state_pub_->publish(is_completed_msg); + } else { + // Switch to next partial trajectory + prev_target_index_ = target_index_; + target_index_ = new_target_index; } } @@ -450,7 +278,7 @@ bool FreespacePlannerNode::isDataReady() void FreespacePlannerNode::onTimer() { scenario_ = scenario_sub_.takeData(); - if (!isActive(scenario_)) { + if (!utils::is_active(scenario_)) { reset(); return; } @@ -463,7 +291,7 @@ void FreespacePlannerNode::onTimer() if (is_completed_) { partial_trajectory_.header = odom_->header; - const auto stop_trajectory = createStopTrajectory(partial_trajectory_); + const auto stop_trajectory = utils::create_stop_trajectory(partial_trajectory_); trajectory_pub_->publish(stop_trajectory); return; } @@ -483,11 +311,11 @@ void FreespacePlannerNode::onTimer() // stops. if (!is_new_parking_cycle_) { const auto stop_trajectory = partial_trajectory_.points.empty() - ? createStopTrajectory(current_pose_) - : createStopTrajectory(partial_trajectory_); + ? utils::create_stop_trajectory(current_pose_) + : utils::create_stop_trajectory(partial_trajectory_); trajectory_pub_->publish(stop_trajectory); - debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); - debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + debug_pose_array_pub_->publish(utils::trajectory_to_pose_array(stop_trajectory)); + debug_partial_pose_array_pub_->publish(utils::trajectory_to_pose_array(stop_trajectory)); } reset(); @@ -496,8 +324,9 @@ void FreespacePlannerNode::onTimer() } if (reset_in_progress_) { - const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); - if (is_stopped) { + const auto is_ego_stopped = + utils::is_stopped(odom_buffer_, node_param_.th_stopped_velocity_mps); + if (is_ego_stopped) { // Plan new trajectory planTrajectory(); reset_in_progress_ = false; @@ -517,12 +346,13 @@ void FreespacePlannerNode::onTimer() // Update partial trajectory updateTargetIndex(); - partial_trajectory_ = getPartialTrajectory(trajectory_, prev_target_index_, target_index_); + partial_trajectory_ = + utils::get_partial_trajectory(trajectory_, prev_target_index_, target_index_); // Publish messages trajectory_pub_->publish(partial_trajectory_); - debug_pose_array_pub_->publish(trajectory2PoseArray(trajectory_)); - debug_partial_pose_array_pub_->publish(trajectory2PoseArray(partial_trajectory_)); + debug_pose_array_pub_->publish(utils::trajectory_to_pose_array(trajectory_)); + debug_partial_pose_array_pub_->publish(utils::trajectory_to_pose_array(partial_trajectory_)); is_new_parking_cycle_ = false; } @@ -537,11 +367,11 @@ void FreespacePlannerNode::planTrajectory() algo_->setMap(*occupancy_grid_); // Calculate poses in costmap frame - const auto current_pose_in_costmap_frame = transformPose( + const auto current_pose_in_costmap_frame = utils::transform_pose( current_pose_.pose, getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); - const auto goal_pose_in_costmap_frame = transformPose( + const auto goal_pose_in_costmap_frame = utils::transform_pose( goal_pose_.pose, getTransform(occupancy_grid_->header.frame_id, goal_pose_.header.frame_id)); // execute planning @@ -559,12 +389,12 @@ void FreespacePlannerNode::planTrajectory() if (result) { RCLCPP_DEBUG(get_logger(), "Found goal!"); - trajectory_ = - createTrajectory(current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity); - reversing_indices_ = getReversingIndices(trajectory_); + trajectory_ = utils::create_trajectory( + current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity); + reversing_indices_ = utils::get_reversing_indices(trajectory_); prev_target_index_ = 0; - target_index_ = - getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_); + target_index_ = utils::get_next_target_index( + trajectory_.points.size(), reversing_indices_, prev_target_index_); } else { RCLCPP_INFO(get_logger(), "Can't find goal: %s", error_msg.c_str()); reset(); diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp new file mode 100644 index 0000000000000..badd9c824468a --- /dev/null +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp @@ -0,0 +1,178 @@ +// 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 "autoware/freespace_planner/utils.hpp" + +#include + +namespace autoware::freespace_planner::utils +{ + +PoseArray trajectory_to_pose_array(const Trajectory & trajectory) +{ + PoseArray pose_array; + pose_array.header = trajectory.header; + + for (const auto & point : trajectory.points) { + pose_array.poses.push_back(point.pose); + } + + return pose_array; +} + +double calc_distance_2d(const Trajectory & trajectory, const Pose & pose) +{ + const auto idx = autoware::motion_utils::findNearestIndex(trajectory.points, pose.position); + return autoware::universe_utils::calcDistance2d(trajectory.points.at(idx), pose); +} + +Pose transform_pose(const Pose & pose, const TransformStamped & transform) +{ + PoseStamped transformed_pose; + PoseStamped orig_pose; + orig_pose.pose = pose; + tf2::doTransform(orig_pose, transformed_pose, transform); + + return transformed_pose.pose; +} + +bool is_active(const Scenario::ConstSharedPtr & scenario) +{ + if (!scenario) return false; + + const auto & s = scenario->activating_scenarios; + return std::find(std::begin(s), std::end(s), Scenario::PARKING) != std::end(s); +} + +std::vector get_reversing_indices(const Trajectory & trajectory) +{ + std::vector indices; + + for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { + if ( + trajectory.points.at(i).longitudinal_velocity_mps * + trajectory.points.at(i + 1).longitudinal_velocity_mps < + 0) { + indices.push_back(i); + } + } + + return indices; +} + +size_t get_next_target_index( + const size_t trajectory_size, const std::vector & reversing_indices, + const size_t current_target_index) +{ + if (!reversing_indices.empty()) { + for (const auto reversing_index : reversing_indices) { + if (reversing_index > current_target_index) { + return reversing_index; + } + } + } + + return trajectory_size - 1; +} + +Trajectory get_partial_trajectory( + const Trajectory & trajectory, const size_t start_index, const size_t end_index) +{ + Trajectory partial_trajectory; + partial_trajectory.header = trajectory.header; + partial_trajectory.header.stamp = rclcpp::Clock().now(); + + partial_trajectory.points.reserve(trajectory.points.size()); + for (size_t i = start_index; i <= end_index; ++i) { + partial_trajectory.points.push_back(trajectory.points.at(i)); + } + + // Modify velocity at start/end point + if (partial_trajectory.points.size() >= 2) { + partial_trajectory.points.front().longitudinal_velocity_mps = + partial_trajectory.points.at(1).longitudinal_velocity_mps; + } + if (!partial_trajectory.points.empty()) { + partial_trajectory.points.back().longitudinal_velocity_mps = 0; + } + + return partial_trajectory; +} + +Trajectory create_trajectory( + const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, + const double & velocity) +{ + Trajectory trajectory; + trajectory.header = planner_waypoints.header; + + for (const auto & awp : planner_waypoints.waypoints) { + TrajectoryPoint point; + + point.pose = awp.pose.pose; + + point.pose.position.z = current_pose.pose.position.z; // height = const + point.longitudinal_velocity_mps = velocity / 3.6; // velocity = const + + // switch sign by forward/backward + point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps; + + trajectory.points.push_back(point); + } + + return trajectory; +} + +Trajectory create_stop_trajectory(const PoseStamped & current_pose) +{ + PlannerWaypoints waypoints; + PlannerWaypoint waypoint; + + waypoints.header.stamp = rclcpp::Clock().now(); + waypoints.header.frame_id = current_pose.header.frame_id; + waypoint.pose.header = waypoints.header; + waypoint.pose.pose = current_pose.pose; + waypoint.is_back = false; + waypoints.waypoints.push_back(waypoint); + + return create_trajectory(current_pose, waypoints, 0.0); +} + +Trajectory create_stop_trajectory(const Trajectory & trajectory) +{ + Trajectory stop_trajectory = trajectory; + for (size_t i = 0; i < trajectory.points.size(); ++i) { + stop_trajectory.points.at(i).longitudinal_velocity_mps = 0.0; + } + return stop_trajectory; +} + +bool is_stopped( + const std::deque & odom_buffer, const double th_stopped_velocity_mps) +{ + for (const auto & odom : odom_buffer) { + if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { + return false; + } + } + return true; +} + +bool is_near_target(const Pose & target_pose, const Pose & current_pose, const double th_distance_m) +{ + const double long_disp_to_target = + autoware::universe_utils::calcLongitudinalDeviation(target_pose, current_pose.position); + return std::abs(long_disp_to_target) < th_distance_m; +} +} // namespace autoware::freespace_planner::utils From 95bb2c61df36d4862432d437b7627a5d8fbc8a9c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 15 Oct 2024 14:51:05 +0000 Subject: [PATCH 24/27] refactor(object_recognition_utils): add autoware prefix to object_recognition_utils (#8946) Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- .../CMakeLists.txt | 6 ++--- .../README.md | 2 +- .../object_recognition_utils/conversion.hpp | 10 ++++---- .../object_recognition_utils/geometry.hpp | 10 ++++---- .../object_recognition_utils/matching.hpp | 12 ++++----- .../object_classification.hpp | 10 ++++---- .../object_recognition_utils.hpp | 25 +++++++++++++++++++ .../predicted_path_utils.hpp | 10 ++++---- .../object_recognition_utils/transform.hpp | 10 ++++---- .../package.xml | 4 +-- .../src/conversion.cpp | 6 ++--- .../src/predicted_path_utils.cpp | 6 ++--- .../test/src/test_conversion.cpp | 6 ++--- .../test/src/test_geometry.cpp | 4 +-- .../test/src/test_matching.cpp | 10 ++++---- .../test/src/test_object_classification.cpp | 20 +++++++-------- .../test/src/test_predicted_path_utils.cpp | 8 +++--- .../object_recognition_utils.hpp | 25 ------------------- .../perception_online_evaluator/package.xml | 2 +- .../src/metrics/detection_count.cpp | 5 ++-- .../src/metrics_calculator.cpp | 6 ++--- .../autoware_cluster_merger/package.xml | 2 +- .../src/cluster_merger_node.cpp | 6 ++--- .../package.xml | 2 +- .../src/lanelet_filter/lanelet_filter.cpp | 4 +-- .../obstacle_pointcloud_validator.cpp | 4 +-- .../occupancy_grid_map_validator.cpp | 10 ++++---- .../autoware_detection_by_tracker/package.xml | 2 +- .../src/detection_by_tracker_node.cpp | 20 +++++++-------- .../package.xml | 2 +- .../src/roi_detected_object_fusion/node.cpp | 11 ++++---- .../postprocess/non_maximum_suppression.cpp | 15 ++++++----- .../lib/ros_utils.cpp | 4 +-- .../autoware_lidar_centerpoint/package.xml | 2 +- .../postprocess/non_maximum_suppression.cpp | 15 ++++++----- .../lib/ros_utils.cpp | 4 +-- .../autoware_lidar_transfusion/package.xml | 2 +- .../tracker/model/tracker_base.hpp | 4 +-- .../uncertainty/uncertainty_processor.hpp | 2 +- .../lib/association/association.cpp | 6 ++--- .../lib/tracker/model/bicycle_tracker.cpp | 8 +++--- .../lib/tracker/model/big_vehicle_tracker.cpp | 8 +++--- .../model/multiple_vehicle_tracker.cpp | 4 ++- .../tracker/model/normal_vehicle_tracker.cpp | 8 +++--- .../tracker/model/pass_through_tracker.cpp | 4 +-- .../model/pedestrian_and_bicycle_tracker.cpp | 4 ++- .../lib/tracker/model/pedestrian_tracker.cpp | 8 +++--- .../lib/tracker/model/unknown_tracker.cpp | 4 +-- .../lib/uncertainty/uncertainty_processor.cpp | 2 +- .../autoware_multi_object_tracker/package.xml | 2 +- .../src/multi_object_tracker_node.cpp | 2 +- .../src/processor/processor.cpp | 8 +++--- perception/autoware_object_merger/package.xml | 2 +- .../src/association/data_association.cpp | 8 +++--- .../src/object_association_merger_node.cpp | 21 +++++++++------- .../package.xml | 2 +- .../src/radar_object_clustering_node.cpp | 4 +-- .../association/data_association.hpp | 2 +- .../tracker/model/tracker_base.hpp | 4 +-- .../autoware_radar_object_tracker/package.xml | 2 +- .../src/association/data_association.cpp | 4 +-- .../src/radar_object_tracker_node.cpp | 8 +++--- .../constant_turn_rate_motion_tracker.cpp | 6 +++-- .../tracker/model/linear_motion_tracker.cpp | 6 +++-- .../tensorrt_yolox/tensorrt_yolox_node.hpp | 2 +- .../autoware_tensorrt_yolox/package.xml | 2 +- .../src/tensorrt_yolox_node.cpp | 6 ++--- .../package.xml | 2 +- .../src/association/data_association.cpp | 9 ++++--- .../src/decorative_tracker_merger_node.cpp | 6 ++--- .../package.xml | 2 +- .../src/node.cpp | 6 ++--- .../src/utils.cpp | 5 ++-- .../package.xml | 2 +- .../src/scene.cpp | 6 ++--- .../src/utils/utils.cpp | 4 +-- .../package.xml | 2 +- .../package.xml | 2 +- .../path_safety_checker/objects_filtering.cpp | 4 +-- .../package.xml | 2 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 2 +- 83 files changed, 273 insertions(+), 240 deletions(-) rename common/{object_recognition_utils => autoware_object_recognition_utils}/CMakeLists.txt (79%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/README.md (87%) rename common/{object_recognition_utils/include => autoware_object_recognition_utils/include/autoware}/object_recognition_utils/conversion.hpp (80%) rename common/{object_recognition_utils/include => autoware_object_recognition_utils/include/autoware}/object_recognition_utils/geometry.hpp (85%) rename common/{object_recognition_utils/include => autoware_object_recognition_utils/include/autoware}/object_recognition_utils/matching.hpp (93%) rename common/{object_recognition_utils/include => autoware_object_recognition_utils/include/autoware}/object_recognition_utils/object_classification.hpp (94%) create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp rename common/{object_recognition_utils/include => autoware_object_recognition_utils/include/autoware}/object_recognition_utils/predicted_path_utils.hpp (88%) rename common/{object_recognition_utils/include => autoware_object_recognition_utils/include/autoware}/object_recognition_utils/transform.hpp (95%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/package.xml (90%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/src/conversion.cpp (94%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/src/predicted_path_utils.cpp (96%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/test/src/test_conversion.cpp (98%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/test/src/test_geometry.cpp (97%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/test/src/test_matching.cpp (97%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/test/src/test_object_classification.cpp (95%) rename common/{object_recognition_utils => autoware_object_recognition_utils}/test/src/test_predicted_path_utils.cpp (97%) delete mode 100644 common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 7594271eae3c5..b00fe3371d11f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -6,6 +6,7 @@ common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +common/autoware_object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_path_distance_calculator/** isamu.takagi@tier4.jp @@ -24,7 +25,6 @@ common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabay common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp -common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp diff --git a/common/object_recognition_utils/CMakeLists.txt b/common/autoware_object_recognition_utils/CMakeLists.txt similarity index 79% rename from common/object_recognition_utils/CMakeLists.txt rename to common/autoware_object_recognition_utils/CMakeLists.txt index 4f05298573c18..9e1276db8d635 100644 --- a/common/object_recognition_utils/CMakeLists.txt +++ b/common/autoware_object_recognition_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(object_recognition_utils) +project(autoware_object_recognition_utils) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) -ament_auto_add_library(object_recognition_utils SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/predicted_path_utils.cpp src/conversion.cpp ) @@ -19,7 +19,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_object_recognition_utils ${test_files}) target_link_libraries(test_object_recognition_utils - object_recognition_utils + ${PROJECT_NAME} ) endif() diff --git a/common/object_recognition_utils/README.md b/common/autoware_object_recognition_utils/README.md similarity index 87% rename from common/object_recognition_utils/README.md rename to common/autoware_object_recognition_utils/README.md index 74a585e2dbd44..8d4eabd19c76d 100644 --- a/common/object_recognition_utils/README.md +++ b/common/autoware_object_recognition_utils/README.md @@ -1,4 +1,4 @@ -# object_recognition_utils +# autoware_object_recognition_utils ## Purpose diff --git a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp similarity index 80% rename from common/object_recognition_utils/include/object_recognition_utils/conversion.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp index 207f76a1c31a8..5b3be48cc2598 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ -#define OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -28,6 +28,6 @@ using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object); DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); TrackedObject toTrackedObject(const DetectedObject & detected_object); -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp similarity index 85% rename from common/object_recognition_utils/include/object_recognition_utils/geometry.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp index 28cc9a786ecac..a5acc210d92be 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ -#define OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ #include #include #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { template geometry_msgs::msg::Pose getPose([[maybe_unused]] const T & p) @@ -52,6 +52,6 @@ inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::Pre { return obj.kinematics.initial_pose_with_covariance.pose; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp similarity index 93% rename from common/object_recognition_utils/include/object_recognition_utils/matching.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp index ae50227df1166..59c01c6d873e7 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ -#define OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#include "autoware/object_recognition_utils/geometry.hpp" #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "object_recognition_utils/geometry.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware::universe_utils::Polygon2d; // minimum area to avoid division by zero @@ -126,6 +126,6 @@ double get2dRecall(const T1 source_object, const T2 target_object) return std::min(1.0, intersection_area / target_area); } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp similarity index 94% rename from common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp index 4ffafc22339d6..432abcbe10c55 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ -#define OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ #include "autoware_perception_msgs/msg/object_classification.hpp" #include #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware_perception_msgs::msg::ObjectClassification; @@ -169,6 +169,6 @@ inline std::string convertLabelToString( return convertLabelToString(highest_prob_label); } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp new file mode 100644 index 0000000000000..d73e2e4dc67db --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp @@ -0,0 +1,25 @@ +// Copyright 2022 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 AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ + +#include "autoware/object_recognition_utils/conversion.hpp" +#include "autoware/object_recognition_utils/geometry.hpp" +#include "autoware/object_recognition_utils/matching.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/transform.hpp" + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp similarity index 88% rename from common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp index 72e70185d553b..836b5296497e8 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ -#define OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ #include "autoware/universe_utils/geometry/geometry.hpp" @@ -24,7 +24,7 @@ #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { /** * @brief Calculate Interpolated Pose from predicted path by time @@ -61,6 +61,6 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp similarity index 95% rename from common/object_recognition_utils/include/object_recognition_utils/transform.hpp rename to common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp index b03270133a52c..ced5956e079e6 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ -#define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #include @@ -71,7 +71,7 @@ namespace detail } } // namespace detail -namespace object_recognition_utils +namespace autoware::object_recognition_utils { template bool transformObjects( @@ -147,6 +147,6 @@ bool transformObjectsWithFeature( } return true; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils -#endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/autoware_object_recognition_utils/package.xml similarity index 90% rename from common/object_recognition_utils/package.xml rename to common/autoware_object_recognition_utils/package.xml index a3f4a99300716..8fcc19ec7a53c 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/autoware_object_recognition_utils/package.xml @@ -1,9 +1,9 @@ - object_recognition_utils + autoware_object_recognition_utils 0.1.0 - The object_recognition_utils package + The autoware_object_recognition_utils package Takayuki Murooka Shunsuke Miura Yoshi Ri diff --git a/common/object_recognition_utils/src/conversion.cpp b/common/autoware_object_recognition_utils/src/conversion.cpp similarity index 94% rename from common/object_recognition_utils/src/conversion.cpp rename to common/autoware_object_recognition_utils/src/conversion.cpp index c9c057ad93644..6f8e6aa27cfb4 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/autoware_object_recognition_utils/src/conversion.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/conversion.hpp" +#include "autoware/object_recognition_utils/conversion.hpp" -namespace object_recognition_utils +namespace autoware::object_recognition_utils { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -68,4 +68,4 @@ TrackedObject toTrackedObject(const DetectedObject & detected_object) tracked_object.shape = detected_object.shape; return tracked_object; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp similarity index 96% rename from common/object_recognition_utils/src/predicted_path_utils.cpp rename to common/autoware_object_recognition_utils/src/predicted_path_utils.cpp index 96c62d9f50323..196f82b69161a 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/interpolation/spherical_linear_interpolation.hpp" @@ -20,7 +20,7 @@ #include -namespace object_recognition_utils +namespace autoware::object_recognition_utils { boost::optional calcInterpolatedPose( const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) @@ -129,4 +129,4 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( resampled_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval); return resampled_path; } -} // namespace object_recognition_utils +} // namespace autoware::object_recognition_utils diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp similarity index 98% rename from common/object_recognition_utils/test/src/test_conversion.cpp rename to common/autoware_object_recognition_utils/test/src/test_conversion.cpp index a9b462f9ec4c4..6a05771149ad2 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/object_recognition_utils/conversion.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/conversion.hpp" #include @@ -42,8 +42,8 @@ autoware_perception_msgs::msg::ObjectClassification createObjectClassification( // NOTE: covariance is not checked TEST(conversion, test_toDetectedObject) { + using autoware::object_recognition_utils::toDetectedObject; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::toDetectedObject; autoware_perception_msgs::msg::TrackedObject tracked_obj; // existence probability @@ -160,8 +160,8 @@ TEST(conversion, test_toDetectedObject) // NOTE: covariance is not checked TEST(conversion, test_toTrackedObject) { + using autoware::object_recognition_utils::toTrackedObject; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::toTrackedObject; autoware_perception_msgs::msg::DetectedObject detected_obj; // existence probability diff --git a/common/object_recognition_utils/test/src/test_geometry.cpp b/common/autoware_object_recognition_utils/test/src/test_geometry.cpp similarity index 97% rename from common/object_recognition_utils/test/src/test_geometry.cpp rename to common/autoware_object_recognition_utils/test/src/test_geometry.cpp index ab3ba8fcec7d4..f5165482a23bf 100644 --- a/common/object_recognition_utils/test/src/test_geometry.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/geometry.hpp" +#include "autoware/object_recognition_utils/geometry.hpp" #include @@ -20,7 +20,7 @@ TEST(geometry, getPose) { - using object_recognition_utils::getPose; + using autoware::object_recognition_utils::getPose; const double x_ans = 1.0; const double y_ans = 2.0; diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/autoware_object_recognition_utils/test/src/test_matching.cpp similarity index 97% rename from common/object_recognition_utils/test/src/test_matching.cpp rename to common/autoware_object_recognition_utils/test/src/test_matching.cpp index 6005ac8d1efbc..aefc6251c6276 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_matching.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/object_recognition_utils/matching.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/matching.hpp" #include @@ -37,8 +37,8 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(matching, test_get2dIoU) { + using autoware::object_recognition_utils::get2dIoU; using autoware_perception_msgs::msg::DetectedObject; - using object_recognition_utils::get2dIoU; const double quart_circle = 0.16237976320958225; @@ -93,7 +93,7 @@ TEST(matching, test_get2dIoU) TEST(object_recognition_utils, test_get2dGeneralizedIoU) { - using object_recognition_utils::get2dGeneralizedIoU; + using autoware::object_recognition_utils::get2dGeneralizedIoU; // TODO(Shin-kyoto): // get2dGeneralizedIoU uses outer points of each polygon. // But these points contain an sampling error of outer line, @@ -153,8 +153,8 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) TEST(matching, test_get2dPrecision) { + using autoware::object_recognition_utils::get2dPrecision; using autoware_perception_msgs::msg::DetectedObject; - using object_recognition_utils::get2dPrecision; const double quart_circle = 0.16237976320958225; { // non overlapped @@ -220,8 +220,8 @@ TEST(matching, test_get2dPrecision) TEST(matching, test_get2dRecall) { + using autoware::object_recognition_utils::get2dRecall; using autoware_perception_msgs::msg::DetectedObject; - using object_recognition_utils::get2dRecall; const double quart_circle = 0.16237976320958225; { // non overlapped diff --git a/common/object_recognition_utils/test/src/test_object_classification.cpp b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp similarity index 95% rename from common/object_recognition_utils/test/src/test_object_classification.cpp rename to common/autoware_object_recognition_utils/test/src/test_object_classification.cpp index 697a7e8447732..1482158581a8e 100644 --- a/common/object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" #include @@ -34,8 +34,8 @@ autoware_perception_msgs::msg::ObjectClassification createObjectClassification( TEST(object_classification, test_getHighestProbLabel) { + using autoware::object_recognition_utils::getHighestProbLabel; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::getHighestProbLabel; { // empty std::vector classifications; @@ -67,8 +67,8 @@ TEST(object_classification, test_getHighestProbLabel) // Test isVehicle TEST(object_classification, test_isVehicle) { + using autoware::object_recognition_utils::isVehicle; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::isVehicle; { // True Case with uint8_t EXPECT_TRUE(isVehicle(ObjectClassification::BICYCLE)); @@ -106,8 +106,8 @@ TEST(object_classification, test_isVehicle) // TEST isCarLikeVehicle TEST(object_classification, test_isCarLikeVehicle) { + using autoware::object_recognition_utils::isCarLikeVehicle; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::isCarLikeVehicle; { // True Case with uint8_t EXPECT_TRUE(isCarLikeVehicle(ObjectClassification::BUS)); @@ -157,8 +157,8 @@ TEST(object_classification, test_isCarLikeVehicle) // TEST isLargeVehicle TEST(object_classification, test_isLargeVehicle) { + using autoware::object_recognition_utils::isLargeVehicle; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::isLargeVehicle; { // True Case with uint8_t EXPECT_TRUE(isLargeVehicle(ObjectClassification::BUS)); @@ -197,8 +197,8 @@ TEST(object_classification, test_isLargeVehicle) TEST(object_classification, test_getHighestProbClassification) { + using autoware::object_recognition_utils::getHighestProbClassification; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::getHighestProbClassification; { // empty std::vector classifications; @@ -232,10 +232,10 @@ TEST(object_classification, test_getHighestProbClassification) TEST(object_classification, test_fromString) { + using autoware::object_recognition_utils::toLabel; + using autoware::object_recognition_utils::toObjectClassification; + using autoware::object_recognition_utils::toObjectClassifications; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::toLabel; - using object_recognition_utils::toObjectClassification; - using object_recognition_utils::toObjectClassifications; // toLabel { @@ -266,8 +266,8 @@ TEST(object_classification, test_fromString) TEST(object_classification, test_convertLabelToString) { + using autoware::object_recognition_utils::convertLabelToString; using autoware_perception_msgs::msg::ObjectClassification; - using object_recognition_utils::convertLabelToString; // from label { diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp similarity index 97% rename from common/object_recognition_utils/test/src/test_predicted_path_utils.cpp rename to common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp index 228a0d097fde6..a93d253bfd052 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include @@ -64,10 +64,10 @@ PredictedPath createTestPredictedPath( TEST(predicted_path_utils, testCalcInterpolatedPose) { + using autoware::object_recognition_utils::calcInterpolatedPose; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::deg2rad; - using object_recognition_utils::calcInterpolatedPose; const auto path = createTestPredictedPath(100, 0.1, 1.0); @@ -130,10 +130,10 @@ TEST(predicted_path_utils, testCalcInterpolatedPose) TEST(predicted_path_utils, resamplePredictedPath_by_vector) { + using autoware::object_recognition_utils::resamplePredictedPath; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::deg2rad; - using object_recognition_utils::resamplePredictedPath; const auto path = createTestPredictedPath(10, 1.0, 1.0); @@ -207,10 +207,10 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) { + using autoware::object_recognition_utils::resamplePredictedPath; using autoware::universe_utils::createQuaternionFromRPY; using autoware::universe_utils::createQuaternionFromYaw; using autoware::universe_utils::deg2rad; - using object_recognition_utils::resamplePredictedPath; const auto path = createTestPredictedPath(10, 1.0, 1.0); diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp deleted file mode 100644 index 4a4ef4f246453..0000000000000 --- a/common/object_recognition_utils/include/object_recognition_utils/object_recognition_utils.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2022 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 OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ -#define OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ - -#include "object_recognition_utils/conversion.hpp" -#include "object_recognition_utils/geometry.hpp" -#include "object_recognition_utils/matching.hpp" -#include "object_recognition_utils/object_classification.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" -#include "object_recognition_utils/transform.hpp" - -#endif // OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 23b1210063705..0103200511e73 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -19,6 +19,7 @@ autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -27,7 +28,6 @@ geometry_msgs glog nav_msgs - object_recognition_utils pluginlib rclcpp rclcpp_components diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp index 5859bf055d4aa..ab69ea9cb8c7b 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp @@ -14,8 +14,8 @@ #include "perception_online_evaluator/metrics/detection_count.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "perception_online_evaluator/utils/objects_filtering.hpp" #include @@ -72,7 +72,8 @@ void DetectionCounter::addObjects( for (const auto & object : objects.objects) { const auto uuid = toHexString(object.object_id); - const auto label = object_recognition_utils::getHighestProbLabel(object.classification); + const auto label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (!isCountObject(label, parameters_->object_parameters)) { continue; } diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 212af8711a62e..fea4316fa6820 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -15,16 +15,16 @@ #include "perception_online_evaluator/metrics_calculator.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_classification.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include namespace perception_diagnostics { +using autoware::object_recognition_utils::convertLabelToString; using autoware::universe_utils::inverseTransformPoint; -using object_recognition_utils::convertLabelToString; std::optional MetricsCalculator::calculate(const Metric & metric) const { diff --git a/perception/autoware_cluster_merger/package.xml b/perception/autoware_cluster_merger/package.xml index 879cf1da2a565..ff3eb60ec9c45 100644 --- a/perception/autoware_cluster_merger/package.xml +++ b/perception/autoware_cluster_merger/package.xml @@ -12,11 +12,11 @@ ament_cmake_auto + autoware_object_recognition_utils autoware_test_utils autoware_universe_utils geometry_msgs message_filters - object_recognition_utils rclcpp rclcpp_components tier4_perception_msgs diff --git a/perception/autoware_cluster_merger/src/cluster_merger_node.cpp b/perception/autoware_cluster_merger/src/cluster_merger_node.cpp index 554196787c028..42e0ef005057f 100644 --- a/perception/autoware_cluster_merger/src/cluster_merger_node.cpp +++ b/perception/autoware_cluster_merger/src/cluster_merger_node.cpp @@ -14,7 +14,7 @@ #include "cluster_merger_node.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -52,9 +52,9 @@ void ClusterMergerNode::objectsCallback( DetectedObjectsWithFeature transformed_objects0; DetectedObjectsWithFeature transformed_objects1; if ( - !object_recognition_utils::transformObjectsWithFeature( + !autoware::object_recognition_utils::transformObjectsWithFeature( *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || - !object_recognition_utils::transformObjectsWithFeature( + !autoware::object_recognition_utils::transformObjectsWithFeature( *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { return; } diff --git a/perception/autoware_detected_object_validation/package.xml b/perception/autoware_detected_object_validation/package.xml index b46c182d2cb57..4f136ad6270c8 100644 --- a/perception/autoware_detected_object_validation/package.xml +++ b/perception/autoware_detected_object_validation/package.xml @@ -19,12 +19,12 @@ autoware_lanelet2_extension autoware_map_msgs + autoware_object_recognition_utils autoware_perception_msgs autoware_test_utils autoware_universe_utils message_filters nav_msgs - object_recognition_utils pcl_conversions rclcpp rclcpp_components diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index 59de872ae9b90..e54ffd5452e7c 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -14,11 +14,11 @@ #include "lanelet_filter.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -97,7 +97,7 @@ void ObjectLaneletFilterNode::objectCallback( return; } autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_msg, lanelet_frame_id_, tf_buffer_, transformed_objects)) { RCLCPP_ERROR(get_logger(), "Failed transform to %s.", lanelet_frame_id_.c_str()); return; diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 7f55c86080fd2..e3889ae8f6513 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -16,8 +16,8 @@ #include "obstacle_pointcloud_validator.hpp" +#include #include -#include #include @@ -325,7 +325,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_, transformed_objects)) { // objects_pub_->publish(*input_objects); diff --git a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index 51df1929a300c..a78e2bf90d8c2 100644 --- a/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp +++ b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -16,9 +16,9 @@ #include "occupancy_grid_map_validator.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "object_recognition_utils/object_classification.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -68,7 +68,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( // Transform to occ grid frame autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_objects, input_occ_grid->header.frame_id, tf_buffer_, transformed_objects)) return; @@ -80,7 +80,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( const auto & transformed_object = transformed_objects.objects.at(i); const auto & object = input_objects->objects.at(i); const auto & label = object.classification.front().label; - if (object_recognition_utils::isCarLikeVehicle(label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(label)) { auto mask = getMask(*input_occ_grid, transformed_object); const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; if (mean_threshold_ < mean) output.objects.push_back(object); @@ -159,7 +159,7 @@ void OccupancyGridBasedValidator::showDebugImage( // Get vehicle mask image and calculate mean within mask. for (const auto & object : objects.objects) { const auto & label = object.classification.front().label; - if (object_recognition_utils::isCarLikeVehicle(label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(label)) { auto mask = getMask(ros_occ_grid, object); const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; if (mean_threshold_ < mean) { diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml index 6f23baeefbd3e..e84e13c484e16 100644 --- a/perception/autoware_detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -14,10 +14,10 @@ eigen3_cmake_module autoware_euclidean_cluster + autoware_object_recognition_utils autoware_shape_estimation autoware_universe_utils eigen - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp index 4c65500d96aaa..571b6682eb5b0 100644 --- a/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp @@ -16,9 +16,9 @@ #include "detection_by_tracker_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -161,14 +161,14 @@ void DetectionByTracker::onObjects( tracker_handler_.estimateTrackedObjects(input_msg->header.stamp, objects); if ( !available_trackers || - !object_recognition_utils::transformObjects( + !autoware::object_recognition_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp); return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. - tracked_objects = object_recognition_utils::toDetectedObjects(transformed_objects); + tracked_objects = autoware::object_recognition_utils::toDetectedObjects(transformed_objects); } debugger_->publishInitialObjects(*input_msg); debugger_->publishTrackedObjects(tracked_objects); @@ -233,9 +233,9 @@ void DetectionByTracker::divideUnderSegmentedObjects( } // detect under segmented cluster const float recall = - object_recognition_utils::get2dRecall(initial_object.object, tracked_object); + autoware::object_recognition_utils::get2dRecall(initial_object.object, tracked_object); const float precision = - object_recognition_utils::get2dPrecision(initial_object.object, tracked_object); + autoware::object_recognition_utils::get2dPrecision(initial_object.object, tracked_object); const bool is_under_segmented = (recall_min_threshold < recall && precision < precision_max_threshold); if (!is_under_segmented) { @@ -310,7 +310,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( if (!is_shape_estimated) { continue; } - const float iou = object_recognition_utils::get2dIoU( + const float iou = autoware::object_recognition_utils::get2dIoU( highest_iou_object_in_current_iter.object, target_object); if (highest_iou_in_current_iter < iou) { highest_iou_in_current_iter = iou; @@ -332,7 +332,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( // build output highest_iou_object.object.classification = target_object.classification; highest_iou_object.object.existence_probability = - object_recognition_utils::get2dIoU(target_object, highest_iou_object.object); + autoware::object_recognition_utils::get2dIoU(target_object, highest_iou_object.object); output = highest_iou_object; return highest_iou; @@ -370,8 +370,8 @@ void DetectionByTracker::mergeOverSegmentedObjects( } // If there is an initial object in the tracker, it will be merged. - const float precision = - object_recognition_utils::get2dPrecision(initial_object.object, extended_tracked_object); + const float precision = autoware::object_recognition_utils::get2dPrecision( + initial_object.object, extended_tracked_object); if (precision < precision_threshold) { continue; } @@ -401,7 +401,7 @@ void DetectionByTracker::mergeOverSegmentedObjects( } feature_object.object.existence_probability = - object_recognition_utils::get2dIoU(tracked_object, feature_object.object); + autoware::object_recognition_utils::get2dIoU(tracked_object, feature_object.object); setClusterInObjectWithFeature(in_cluster_objects.header, pcl_merged_cluster, feature_object); out_objects.feature_objects.push_back(feature_object); } diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index f08ab04bc6616..b494f2d13887d 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -18,6 +18,7 @@ autoware_euclidean_cluster autoware_lidar_centerpoint + autoware_object_recognition_utils autoware_perception_msgs autoware_point_types autoware_universe_utils @@ -25,7 +26,6 @@ image_geometry image_transport message_filters - object_recognition_utils perception_utils rclcpp rclcpp_components diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 39835b465d8af..043e648403f3b 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -14,7 +14,7 @@ #include "autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -50,8 +50,9 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & object = output_msg.objects.at(obj_i); - const auto label = object_recognition_utils::getHighestProbLabel(object.classification); - const auto pos = object_recognition_utils::getPose(object).position; + const auto label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); + const auto pos = autoware::object_recognition_utils::getPose(object).position; const auto object_sqr_dist = pos.x * pos.x + pos.y * pos.y; const auto prob_threshold = fusion_params_.passthrough_lower_bound_probability_thresholds.at(label); @@ -222,9 +223,9 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( for (const auto & image_roi : image_rois) { const auto & object_roi = object_pair.second; const auto object_roi_label = - object_recognition_utils::getHighestProbLabel(object_roi.object.classification); + autoware::object_recognition_utils::getHighestProbLabel(object_roi.object.classification); const auto image_roi_label = - object_recognition_utils::getHighestProbLabel(image_roi.object.classification); + autoware::object_recognition_utils::getHighestProbLabel(image_roi.object.classification); if (!fusion_params_.can_assign_matrix(object_roi_label, image_roi_label)) { continue; } diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index f7dd85b019250..8f3f0fa9b20e2 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include +#include #include -#include -#include namespace autoware::lidar_centerpoint { @@ -41,8 +41,10 @@ bool NonMaximumSuppression::isTargetLabel(const uint8_t label) bool NonMaximumSuppression::isTargetPairObject( const DetectedObject & object1, const DetectedObject & object2) { - const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); - const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + const auto label1 = + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = + autoware::object_recognition_utils::getHighestProbLabel(object2.classification); if (isTargetLabel(label1) && isTargetLabel(label2)) { return true; @@ -50,7 +52,8 @@ bool NonMaximumSuppression::isTargetPairObject( const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( - object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + autoware::object_recognition_utils::getPose(object1), + autoware::object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } @@ -69,7 +72,7 @@ Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( } if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { - const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + const double iou = autoware::object_recognition_utils::get2dIoU(target_obj, source_obj); triangular_matrix(target_i, source_i) = iou; // NOTE: If the target object has any objects with iou > iou_threshold, it // will be suppressed regardless of later results. diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index feeab969e88bd..c76fa6567ab51 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_centerpoint/ros_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/constants.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" namespace autoware::lidar_centerpoint { @@ -41,7 +41,7 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_centerpoint"), "Unexpected label: UNKNOWN is set."); } - if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(classification.label)) { obj.kinematics.orientation_availability = autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; } diff --git a/perception/autoware_lidar_centerpoint/package.xml b/perception/autoware_lidar_centerpoint/package.xml index 07f67f7e3c11e..29a7bc122707e 100644 --- a/perception/autoware_lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -12,9 +12,9 @@ ament_cmake_python autoware_cmake + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils - object_recognition_utils pcl_ros rclcpp rclcpp_components diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index 6e563f706e3b2..1ce151d2960ef 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include +#include #include -#include -#include namespace autoware::lidar_transfusion { @@ -41,8 +41,10 @@ bool NonMaximumSuppression::isTargetLabel(const uint8_t label) bool NonMaximumSuppression::isTargetPairObject( const DetectedObject & object1, const DetectedObject & object2) { - const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); - const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + const auto label1 = + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = + autoware::object_recognition_utils::getHighestProbLabel(object2.classification); if (isTargetLabel(label1) && isTargetLabel(label2)) { return true; @@ -50,7 +52,8 @@ bool NonMaximumSuppression::isTargetPairObject( const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( - object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + autoware::object_recognition_utils::getPose(object1), + autoware::object_recognition_utils::getPose(object2)); return sqr_dist_2d <= search_sqr_dist_2d; } @@ -69,7 +72,7 @@ Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( } if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { - const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + const double iou = autoware::object_recognition_utils::get2dIoU(target_obj, source_obj); triangular_matrix(target_i, source_i) = iou; // NOTE: If the target object has any objects with iou > iou_threshold, it // will be suppressed regardless of later results. diff --git a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp index ce2a8d1911941..88f6497f8d656 100644 --- a/perception/autoware_lidar_transfusion/lib/ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp @@ -14,9 +14,9 @@ #include "autoware/lidar_transfusion/ros_utils.hpp" +#include #include #include -#include namespace autoware::lidar_transfusion { @@ -40,7 +40,7 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_transfusion"), "Unexpected label: UNKNOWN is set."); } - if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + if (autoware::object_recognition_utils::isCarLikeVehicle(classification.label)) { obj.kinematics.orientation_availability = autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; } diff --git a/perception/autoware_lidar_transfusion/package.xml b/perception/autoware_lidar_transfusion/package.xml index 834afba1221ad..791832ef44bda 100644 --- a/perception/autoware_lidar_transfusion/package.xml +++ b/perception/autoware_lidar_transfusion/package.xml @@ -12,11 +12,11 @@ ament_cmake_auto autoware_cmake + autoware_object_recognition_utils autoware_perception_msgs autoware_point_types autoware_universe_utils launch_ros - object_recognition_utils pcl_ros rclcpp rclcpp_components diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index bc52e49f763fd..0b9ea9c44a6cf 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -21,7 +21,7 @@ #define EIGEN_MPL2_ONLY #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -79,7 +79,7 @@ class Tracker } std::uint8_t getHighestProbLabel() const { - return object_recognition_utils::getHighestProbLabel(classification_); + return autoware::object_recognition_utils::getHighestProbLabel(classification_); } // existence states diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index 52781be1b8201..c67e71b38a6f9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -20,7 +20,7 @@ #include "autoware/multi_object_tracker/object_model/object_model.hpp" -#include +#include #include diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index ac31cad4cc13c..0c809ee5f086d 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -16,7 +16,7 @@ #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -165,7 +165,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::DetectedObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = - object_recognition_utils::getHighestProbLabel(measurement_object.classification); + autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { @@ -210,7 +210,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = object_recognition_utils::get2dIoU( + const double iou = autoware::object_recognition_utils::get2dIoU( measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 38a49e48d8d10..098ad39dd3df9 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -246,7 +246,9 @@ bool BicycleTracker::measure( // update classification const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -272,7 +274,7 @@ bool BicycleTracker::measure( bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index d5913eccdf0bf..349ffb1eec634 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -301,7 +301,9 @@ bool BigVehicleTracker::measure( // update classification const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -327,7 +329,7 @@ bool BigVehicleTracker::measure( bool BigVehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index a6e5021e4fe7d..4751e3d1c894f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -48,7 +48,9 @@ bool MultipleVehicleTracker::measure( { big_vehicle_tracker_.measure(object, time, self_transform); normal_vehicle_tracker_.measure(object, time, self_transform); - if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) != + Label::UNKNOWN) updateClassification(object.classification); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index a9260201722b3..b4be5e42b0397 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -303,7 +303,9 @@ bool NormalVehicleTracker::measure( // update classification const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -329,7 +331,7 @@ bool NormalVehicleTracker::measure( bool NormalVehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index c872944fab3d7..f66e616241ae0 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -19,8 +19,8 @@ #include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -91,7 +91,7 @@ bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 8c665e6078a2b..1b8018351f5a5 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -48,7 +48,9 @@ bool PedestrianAndBicycleTracker::measure( { pedestrian_tracker_.measure(object, time, self_transform); bicycle_tracker_.measure(object, time, self_transform); - if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) != + Label::UNKNOWN) updateClassification(object.classification); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 591453a14f116..2135514df8485 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -20,11 +20,11 @@ #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -242,7 +242,9 @@ bool PedestrianTracker::measure( object_ = object; const auto & current_classification = getClassification(); - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -269,7 +271,7 @@ bool PedestrianTracker::measure( bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 0648007e0b807..2805e43b88323 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -34,7 +34,7 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" namespace autoware::multi_object_tracker { @@ -217,7 +217,7 @@ bool UnknownTracker::measure( bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index 6770b298bdbda..840879f9bd7aa 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -108,7 +108,7 @@ DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) continue; } const ObjectClassification & object_class = - object_recognition_utils::getHighestProbClassification(object.classification); + autoware::object_recognition_utils::getHighestProbClassification(object.classification); updating_objects.objects.push_back(modelUncertaintyByClass(object, object_class)); } return updating_objects; diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml index 405ce7eced625..d7250293df28e 100644 --- a/perception/autoware_multi_object_tracker/package.xml +++ b/perception/autoware_multi_object_tracker/package.xml @@ -14,13 +14,13 @@ eigen3_cmake_module autoware_kalman_filter + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils diagnostic_updater eigen glog mussp - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index e5ffddb31ccce..24bf4ef9c123d 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -282,7 +282,7 @@ void MultiObjectTracker::runProcess( // Transform the objects to the world frame DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( input_objects_with_uncertainty, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 17871c89b5258..ba9b3d17fd20f 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -15,7 +15,7 @@ #include "processor.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware_perception_msgs/msg/tracked_objects.hpp" @@ -90,7 +90,8 @@ std::shared_ptr TrackerProcessor::createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { - const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + const std::uint8_t label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); if (tracker == "bicycle_tracker") @@ -168,7 +169,8 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) // Check the Intersection over Union (IoU) between the two objects const double min_union_iou_area = 1e-2; - const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + const auto iou = + autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; diff --git a/perception/autoware_object_merger/package.xml b/perception/autoware_object_merger/package.xml index e87ef8d81b01b..77c30aa6d4556 100644 --- a/perception/autoware_object_merger/package.xml +++ b/perception/autoware_object_merger/package.xml @@ -13,11 +13,11 @@ autoware_cmake eigen3_cmake_module + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils eigen mussp - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_object_merger/src/association/data_association.cpp b/perception/autoware_object_merger/src/association/data_association.cpp index 8b40178b592f8..bec2d48469562 100644 --- a/perception/autoware_object_merger/src/association/data_association.cpp +++ b/perception/autoware_object_merger/src/association/data_association.cpp @@ -15,8 +15,8 @@ #include "autoware/object_merger/association/data_association.hpp" #include "autoware/object_merger/association/solver/gnn_solver.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -124,13 +124,13 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::DetectedObject & object1 = objects1.objects.at(objects1_idx); const std::uint8_t object1_label = - object_recognition_utils::getHighestProbLabel(object1.classification); + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { const autoware_perception_msgs::msg::DetectedObject & object0 = objects0.objects.at(objects0_idx); const std::uint8_t object0_label = - object_recognition_utils::getHighestProbLabel(object0.classification); + autoware::object_recognition_utils::getHighestProbLabel(object0.classification); double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { @@ -158,7 +158,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const double min_iou = min_iou_matrix_(object1_label, object0_label); const double min_union_iou_area = 1e-2; const double iou = - object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + autoware::object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 663a338649d3c..af9d0e15f4894 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -16,8 +16,8 @@ #include "autoware/object_merger/object_association_merger_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -40,18 +40,19 @@ bool isUnknownObjectOverlapped( const std::map & generalized_iou_threshold_map) { const double generalized_iou_threshold = generalized_iou_threshold_map.at( - object_recognition_utils::getHighestProbLabel(known_object.classification)); + autoware::object_recognition_utils::getHighestProbLabel(known_object.classification)); const double distance_threshold = distance_threshold_map.at( - object_recognition_utils::getHighestProbLabel(known_object.classification)); + autoware::object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( unknown_object.kinematics.pose_with_covariance.pose, known_object.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; - const auto precision = object_recognition_utils::get2dPrecision(unknown_object, known_object); - const auto recall = object_recognition_utils::get2dRecall(unknown_object, known_object); + const auto precision = + autoware::object_recognition_utils::get2dPrecision(unknown_object, known_object); + const auto recall = autoware::object_recognition_utils::get2dRecall(unknown_object, known_object); const auto generalized_iou = - object_recognition_utils::get2dGeneralizedIoU(unknown_object, known_object); + autoware::object_recognition_utils::get2dGeneralizedIoU(unknown_object, known_object); return precision > precision_threshold || recall > recall_threshold || generalized_iou > generalized_iou_threshold; } @@ -142,9 +143,9 @@ void ObjectAssociationMergerNode::objectsCallback( /* transform to base_link coordinate */ autoware_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1; if ( - !object_recognition_utils::transformObjects( + !autoware::object_recognition_utils::transformObjects( *input_objects0_msg, base_link_frame_id_, tf_buffer_, transformed_objects0) || - !object_recognition_utils::transformObjects( + !autoware::object_recognition_utils::transformObjects( *input_objects1_msg, base_link_frame_id_, tf_buffer_, transformed_objects1)) { return; } @@ -198,7 +199,9 @@ void ObjectAssociationMergerNode::objectsCallback( unknown_objects.reserve(output_msg.objects.size()); known_objects.reserve(output_msg.objects.size()); for (const auto & object : output_msg.objects) { - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { unknown_objects.push_back(object); } else { known_objects.push_back(object); diff --git a/perception/autoware_radar_object_clustering/package.xml b/perception/autoware_radar_object_clustering/package.xml index d9c3464e02b18..00106ee38e82f 100644 --- a/perception/autoware_radar_object_clustering/package.xml +++ b/perception/autoware_radar_object_clustering/package.xml @@ -14,10 +14,10 @@ ament_cmake_auto + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils geometry_msgs - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp index 4db4c578f9f89..cf686e3ae1055 100644 --- a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp +++ b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp @@ -14,9 +14,9 @@ #include "radar_object_clustering_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -162,7 +162,7 @@ void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr // Fixed label correction if (node_param_.is_fixed_label) { clustered_output_object.classification.at(0).label = - object_recognition_utils::toLabel(node_param_.fixed_label); + autoware::object_recognition_utils::toLabel(node_param_.fixed_label); } // Fixed size correction diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp index efde1e6763cdd..ad53f8f9d7bf0 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp @@ -21,9 +21,9 @@ #define EIGEN_MPL2_ONLY +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware_radar_object_tracker/association/solver/gnn_solver.hpp" #include "autoware_radar_object_tracker/tracker/tracker.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp index caea725ef8f81..cc51af599cdd9 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp @@ -20,8 +20,8 @@ #define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware_radar_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -68,7 +68,7 @@ class Tracker } std::uint8_t getHighestProbLabel() const { - return object_recognition_utils::getHighestProbLabel(classification_); + return autoware::object_recognition_utils::getHighestProbLabel(classification_); } int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } diff --git a/perception/autoware_radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml index 6829c26f797c7..3a18b93451d39 100644 --- a/perception/autoware_radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -15,13 +15,13 @@ autoware_kalman_filter autoware_lanelet2_extension + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils eigen glog mussp nlohmann-json-dev - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_radar_object_tracker/src/association/data_association.cpp b/perception/autoware_radar_object_tracker/src/association/data_association.cpp index 2d906ce55bcec..673ae084fb242 100644 --- a/perception/autoware_radar_object_tracker/src/association/data_association.cpp +++ b/perception/autoware_radar_object_tracker/src/association/data_association.cpp @@ -176,7 +176,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const autoware_perception_msgs::msg::DetectedObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = - object_recognition_utils::getHighestProbLabel(measurement_object.classification); + autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); // Create a JSON object to hold the log data for this pair nlohmann::json pair_log_data; @@ -259,7 +259,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = object_recognition_utils::get2dIoU( + const double iou = autoware::object_recognition_utils::get2dIoU( measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) { passed_gate = false; diff --git a/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp index 10bf11e46abb9..1815b8ed64b04 100644 --- a/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp +++ b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp @@ -150,7 +150,7 @@ void RadarObjectTrackerNode::onMeasurement( /* transform to world coordinate */ autoware_perception_msgs::msg::DetectedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -215,7 +215,8 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & /*self_transform*/) const { - const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + const std::uint8_t label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); @@ -347,7 +348,8 @@ void RadarObjectTrackerNode::sanitizeTracker( } const double min_union_iou_area = 1e-2; - const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + const auto iou = + autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index f30e4f984f8a1..47b9430616fc9 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -497,7 +497,9 @@ bool ConstantTurnRateMotionTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -517,7 +519,7 @@ bool ConstantTurnRateMotionTracker::measure( bool ConstantTurnRateMotionTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index dbadb3366ba0c..6facac3707c56 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -516,7 +516,9 @@ bool LinearMotionTracker::measure( { const auto & current_classification = getClassification(); object_ = object; - if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) == + Label::UNKNOWN) { setClassification(current_classification); } @@ -536,7 +538,7 @@ bool LinearMotionTracker::measure( bool LinearMotionTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { - object = object_recognition_utils::toTrackedObject(object_); + object = autoware::object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp index 2e317139f27c0..905957178ebd6 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ #define AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ -#include "object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include #include diff --git a/perception/autoware_tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml index cd9dc6bac015d..749624bcbe5a3 100644 --- a/perception/autoware_tensorrt_yolox/package.xml +++ b/perception/autoware_tensorrt_yolox/package.xml @@ -19,12 +19,12 @@ cudnn_cmake_module tensorrt_cmake_module + autoware_object_recognition_utils autoware_perception_msgs cuda_utils cv_bridge image_transport libopencv-dev - object_recognition_utils perception_utils rclcpp rclcpp_components diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index 2455411618ceb..753e51d51265c 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -14,7 +14,7 @@ #include "autoware/tensorrt_yolox/tensorrt_yolox_node.hpp" -#include "object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" #include "perception_utils/run_length_encoder.hpp" #include @@ -168,8 +168,8 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) object.feature.roi.width = yolox_object.width; object.feature.roi.height = yolox_object.height; object.object.existence_probability = yolox_object.score; - object.object.classification = - object_recognition_utils::toObjectClassifications(label_map_[yolox_object.type], 1.0f); + object.object.classification = autoware::object_recognition_utils::toObjectClassifications( + label_map_[yolox_object.type], 1.0f); out_objects.feature_objects.push_back(object); const auto left = std::max(0, static_cast(object.feature.roi.x_offset)); const auto top = std::max(0, static_cast(object.feature.roi.y_offset)); diff --git a/perception/autoware_tracking_object_merger/package.xml b/perception/autoware_tracking_object_merger/package.xml index 321016ac0ff48..ce4cf5276ab80 100644 --- a/perception/autoware_tracking_object_merger/package.xml +++ b/perception/autoware_tracking_object_merger/package.xml @@ -13,12 +13,12 @@ autoware_cmake eigen3_cmake_module + autoware_object_recognition_utils autoware_perception_msgs autoware_universe_utils eigen glog mussp - object_recognition_utils rclcpp rclcpp_components tf2 diff --git a/perception/autoware_tracking_object_merger/src/association/data_association.cpp b/perception/autoware_tracking_object_merger/src/association/data_association.cpp index 119c95c86daf8..771fcb5f1a484 100644 --- a/perception/autoware_tracking_object_merger/src/association/data_association.cpp +++ b/perception/autoware_tracking_object_merger/src/association/data_association.cpp @@ -14,10 +14,10 @@ #include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -178,9 +178,9 @@ double DataAssociation::calcScoreBetweenObjects( const autoware_perception_msgs::msg::TrackedObject & object1) const { const std::uint8_t object1_label = - object_recognition_utils::getHighestProbLabel(object1.classification); + autoware::object_recognition_utils::getHighestProbLabel(object1.classification); const std::uint8_t object0_label = - object_recognition_utils::getHighestProbLabel(object0.classification); + autoware::object_recognition_utils::getHighestProbLabel(object0.classification); double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { @@ -206,7 +206,8 @@ double DataAssociation::calcScoreBetweenObjects( if (passed_gate) { const double min_iou = min_iou_matrix_(object1_label, object0_label); const double min_union_iou_area = 1e-2; - const double iou = object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + const double iou = + autoware::object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); if (iou < min_iou) passed_gate = false; } // max velocity diff gate diff --git a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 295a350071ac8..55d31a1b890cd 100644 --- a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -16,9 +16,9 @@ #include "autoware/tracking_object_merger/decorative_tracker_merger_node.hpp" +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "autoware/tracking_object_merger/association/solver/ssp.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -200,7 +200,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( /* transform to target merge coordinate */ TrackedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *main_objects, merge_frame_id_, tf_buffer_, transformed_objects)) { return; } @@ -258,7 +258,7 @@ void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::Const { /* transform to target merge coordinate */ TrackedObjects transformed_objects; - if (!object_recognition_utils::transformObjects( + if (!autoware::object_recognition_utils::transformObjects( *msg, merge_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index ed3cd2169e343..d1c3b8b00e069 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -22,6 +22,7 @@ autoware_interpolation autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_osqp_interface autoware_perception_msgs autoware_planning_msgs @@ -31,7 +32,6 @@ autoware_vehicle_info_utils geometry_msgs nav_msgs - object_recognition_utils rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index f5cac52a68681..37b82ecb66e74 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -16,12 +16,12 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -99,7 +99,7 @@ std::vector resampleHighestConfidencePredictedPaths( continue; } resampled_paths.push_back( - object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); } return resampled_paths; @@ -1758,7 +1758,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacleForPred if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) { return false; } - const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose( + const auto future_obj_pose = autoware::object_recognition_utils::calcInterpolatedPose( resampled_predicted_paths.front(), time_to_reach_stop_point - p.collision_time_margin); Obstacle tmp_future_obs = obstacle; diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 8162d8036d9dc..bc52e800c8fc5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -14,8 +14,8 @@ #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" namespace obstacle_cruise_utils { @@ -30,7 +30,8 @@ std::optional getCurrentObjectPoseFromPredictedPath( return std::nullopt; } - const auto pose = object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); + const auto pose = + autoware::object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); if (!pose) { return std::nullopt; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 9308aab7642f5..858bc78de09eb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -20,6 +20,7 @@ autoware_behavior_path_planner_common autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_planning_msgs autoware_signal_processing @@ -27,7 +28,6 @@ autoware_vehicle_msgs geometry_msgs lanelet2_core - object_recognition_utils pluginlib rclcpp tf2 diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index 3fa3f04435e87..852a3fe0feedf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -16,9 +16,9 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -796,7 +796,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje return is_object_left; } const auto future_obj_pose = - object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); + autoware::object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); const size_t future_obj_idx = autoware::motion_utils::findNearestIndex(input_path.points, future_obj_pose->position); @@ -1001,7 +1001,7 @@ DynamicObstacleAvoidanceModule::calcCollisionSection( const auto future_ego_pose = ego_path.at(i); const auto future_obj_pose = - object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); + autoware::object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); if (future_obj_pose) { const double dist_ego_to_obj = diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index c4fe5fa9902c5..82a832d30c0b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -24,8 +24,8 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -990,7 +990,7 @@ ExtendedPredictedObject transform( if (t < prepare_duration && obj_vel_norm < velocity_threshold) { continue; } - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 6196a4f02cff9..1c3509bc22482 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -44,6 +44,7 @@ autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_path_sampler autoware_perception_msgs autoware_planning_msgs @@ -57,7 +58,6 @@ libboost-dev libopencv-dev magic_enum - object_recognition_utils pluginlib rclcpp rclcpp_components diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index 28b79b502599d..d98e832e23e00 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -48,6 +48,7 @@ autoware_lane_departure_checker autoware_lanelet2_extension autoware_motion_utils + autoware_object_recognition_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs autoware_planning_msgs @@ -57,7 +58,6 @@ autoware_vehicle_info_utils geometry_msgs magic_enum - object_recognition_utils rclcpp tf2 tier4_planning_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 20221fff82175..f183f5160cea8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -15,7 +15,7 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -336,7 +336,7 @@ ExtendedPredictedObject transform( // Create path based on time horizon and resolution for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths[i].path.emplace_back( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index ce6cf92fc6450..b8fc098826e7a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common autoware_motion_utils + autoware_object_recognition_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -31,7 +32,6 @@ geometry_msgs libboost-dev message_filters - object_recognition_utils pcl_conversions pluginlib rclcpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 0a38e8d47ea36..5a219f654561a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -37,7 +37,7 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using object_recognition_utils::convertLabelToString; +using autoware::object_recognition_utils::convertLabelToString; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 7d8513485ac52..4f326aef298be 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ +#include "autoware/object_recognition_utils/object_recognition_utils.hpp" #include "debug.hpp" #include "dynamic_obstacle.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include "state_machine.hpp" #include "utils.hpp" From bc1e8d5c0cc17c971905055f045c2d13ed05c3ee Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 16 Oct 2024 09:24:30 +0900 Subject: [PATCH 25/27] test(freespace_planner): add unit tests for util functions (#9059) * refactor freespace planner Signed-off-by: mohammad alqudah * add function is_near_target to freespace planner utils Signed-off-by: mohammad alqudah * add freespace planner utils namespace Signed-off-by: mohammad alqudah * fix function call Signed-off-by: mohammad alqudah * implement unit tests for freespace planner util functions Signed-off-by: mohammad alqudah * add freespace planner utils namespace Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../autoware_freespace_planner/CMakeLists.txt | 1 + .../test/test_freespace_planner_utils.cpp | 263 ++++++++++++++++++ 2 files changed, 264 insertions(+) create mode 100644 planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp diff --git a/planning/autoware_freespace_planner/CMakeLists.txt b/planning/autoware_freespace_planner/CMakeLists.txt index bbbb09c7d5af1..12ec72d53d854 100644 --- a/planning/autoware_freespace_planner/CMakeLists.txt +++ b/planning/autoware_freespace_planner/CMakeLists.txt @@ -17,6 +17,7 @@ rclcpp_components_register_node(freespace_planner_node if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_freespace_planner_node_interface.cpp + test/test_freespace_planner_utils.cpp ) target_link_libraries(test_${PROJECT_NAME} freespace_planner_node diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp new file mode 100644 index 0000000000000..bccff30e11583 --- /dev/null +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp @@ -0,0 +1,263 @@ +// 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 "autoware/freespace_planner/utils.hpp" + +#include + +#include + +#include +#include + +using autoware::freespace_planner::utils::Odometry; +using autoware::freespace_planner::utils::Scenario; +using autoware::freespace_planner::utils::Trajectory; +using autoware::freespace_planner::utils::TrajectoryPoint; +using autoware::freespace_planning_algorithms::PlannerWaypoint; +using autoware::freespace_planning_algorithms::PlannerWaypoints; + +Trajectory get_trajectory(const size_t n_switches) +{ + const auto get_traj_point = [](const double x, const double y, const float v) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = v; + return point; + }; + + Trajectory trajectory; + constexpr double eps = 0.001; + constexpr double res = 0.5; + + const auto add_points = [&](const double length, const bool forward) { + auto x = trajectory.points.empty() ? 0.0 : trajectory.points.back().pose.position.x; + + if (forward) { + const auto target = x + length; + for (; x < target + eps; x += res) { + trajectory.points.push_back(get_traj_point(x, 0.0, 1.0)); + } + } else { + const auto target = x - length; + for (; x > target - eps; x -= res) { + trajectory.points.push_back(get_traj_point(x, 0.0, -1.0)); + } + } + }; + + bool forward = true; + for (size_t i = 0; i <= n_switches; ++i) { + const auto length = forward ? 5.0 : 3.0; + add_points(length, forward); + forward = !forward; + } + + return trajectory; +} + +PlannerWaypoints get_waypoints(const size_t n_switches) +{ + const auto get_waypoint = [](const double x, const double y, const bool is_back) { + PlannerWaypoint point; + point.pose.pose.position.x = x; + point.pose.pose.position.y = y; + point.is_back = is_back; + return point; + }; + + PlannerWaypoints waypoints; + constexpr double eps = 0.001; + constexpr double res = 0.5; + + const auto add_points = [&](const double length, const bool forward) { + auto x = waypoints.waypoints.empty() ? 0.0 : waypoints.waypoints.back().pose.pose.position.x; + + if (forward) { + const auto target = x + length; + for (; x < target + eps; x += res) { + waypoints.waypoints.push_back(get_waypoint(x, 0.0, !forward)); + } + } else { + const auto target = x - length; + for (; x > target - eps; x -= res) { + waypoints.waypoints.push_back(get_waypoint(x, 0.0, !forward)); + } + } + }; + + bool forward = true; + for (size_t i = 0; i <= n_switches; ++i) { + const auto length = forward ? 5.0 : 3.0; + add_points(length, forward); + forward = !forward; + } + + return waypoints; +} + +TEST(FreespacePlannerUtilsTest, testIsActive) +{ + Scenario::ConstSharedPtr scenario_ptr; + + EXPECT_FALSE(autoware::freespace_planner::utils::is_active(scenario_ptr)); + + Scenario scenario; + scenario.current_scenario = Scenario::EMPTY; + scenario_ptr = std::make_shared(scenario); + EXPECT_FALSE(autoware::freespace_planner::utils::is_active(scenario_ptr)); + + scenario.current_scenario = Scenario::LANEDRIVING; + scenario_ptr = std::make_shared(scenario); + EXPECT_FALSE(autoware::freespace_planner::utils::is_active(scenario_ptr)); + + scenario.current_scenario = Scenario::PARKING; + scenario.activating_scenarios.push_back(Scenario::PARKING); + scenario_ptr = std::make_shared(scenario); + EXPECT_TRUE(autoware::freespace_planner::utils::is_active(scenario_ptr)); +} + +TEST(FreespacePlannerUtilsTest, testIsStopped) +{ + std::deque odometry_buffer; + const double th_stopped_velocity_mps = 0.01; + EXPECT_TRUE( + autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); + + Odometry odometry; + odometry.twist.twist.linear.x = 0.0; + odometry_buffer.push_back(std::make_shared(odometry)); + EXPECT_TRUE( + autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); + + odometry.twist.twist.linear.x = 1.0; + odometry_buffer.push_back(std::make_shared(odometry)); + EXPECT_FALSE( + autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); +} + +TEST(FreespacePlannerUtilsTest, testGetReversingIndices) +{ + auto trajectory = get_trajectory(0ul); + auto reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + EXPECT_EQ(reversing_indices.size(), 0ul); + + trajectory = get_trajectory(1ul); + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + EXPECT_EQ(reversing_indices.size(), 1ul); + if (!reversing_indices.empty()) { + EXPECT_EQ(reversing_indices.front(), 10ul); + } + + trajectory = get_trajectory(2ul); + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + EXPECT_EQ(reversing_indices.size(), 2ul); + if (!reversing_indices.empty()) { + EXPECT_EQ(reversing_indices.front(), 10ul); + EXPECT_EQ(reversing_indices.back(), 17ul); + } +} + +TEST(FreespacePlannerUtilsTest, testGetNextTargetIndex) +{ + auto trajectory = get_trajectory(0ul); + auto reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + auto current_target_index = 0ul; + auto next_target_index = autoware::freespace_planner::utils::get_next_target_index( + trajectory.points.size(), reversing_indices, current_target_index); + EXPECT_EQ(next_target_index, trajectory.points.size() - 1); + + trajectory = get_trajectory(2ul); + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory); + ASSERT_EQ(reversing_indices.size(), 2ul); + + next_target_index = autoware::freespace_planner::utils::get_next_target_index( + trajectory.points.size(), reversing_indices, current_target_index); + EXPECT_EQ(next_target_index, reversing_indices.front()); + + current_target_index = reversing_indices.front(); + next_target_index = autoware::freespace_planner::utils::get_next_target_index( + trajectory.points.size(), reversing_indices, current_target_index); + EXPECT_EQ(next_target_index, reversing_indices.back()); +} + +TEST(FreespacePlannerUtilsTest, testGetPartialTrajectory) +{ + const auto trajectory = get_trajectory(2ul); + const auto reversing_indices = + autoware::freespace_planner::utils::get_reversing_indices(trajectory); + + ASSERT_EQ(reversing_indices.size(), 2ul); + + auto partial_traj = autoware::freespace_planner::utils::get_partial_trajectory( + trajectory, 0ul, reversing_indices.front()); + ASSERT_FALSE(partial_traj.points.empty()); + auto expected_size = reversing_indices.front() + 1ul; + EXPECT_EQ(partial_traj.points.size(), expected_size); + EXPECT_TRUE(partial_traj.points.front().longitudinal_velocity_mps > 0.0); + EXPECT_FLOAT_EQ(partial_traj.points.back().longitudinal_velocity_mps, 0.0); + + partial_traj = autoware::freespace_planner::utils::get_partial_trajectory( + trajectory, reversing_indices.front(), reversing_indices.back()); + ASSERT_FALSE(partial_traj.points.empty()); + expected_size = reversing_indices.back() - reversing_indices.front() + 1ul; + EXPECT_EQ(partial_traj.points.size(), expected_size); + EXPECT_TRUE(partial_traj.points.front().longitudinal_velocity_mps < 0.0); + EXPECT_FLOAT_EQ(partial_traj.points.back().longitudinal_velocity_mps, 0.0); +} + +TEST(FreespacePlannerUtilsTest, testCreateTrajectory) +{ + const auto waypoints = get_waypoints(1ul); + + geometry_msgs::msg::PoseStamped current_pose; + current_pose.pose.position.z = 1.0; + const double velocity = 1.0; + + const auto trajectory = + autoware::freespace_planner::utils::create_trajectory(current_pose, waypoints, velocity); + + ASSERT_FALSE(trajectory.points.empty()); + EXPECT_EQ(trajectory.points.size(), waypoints.waypoints.size()); + EXPECT_EQ(trajectory.points.front().pose.position.z, current_pose.pose.position.z); + EXPECT_EQ(trajectory.points.back().pose.position.z, current_pose.pose.position.z); + EXPECT_TRUE(trajectory.points.front().longitudinal_velocity_mps > 0.0); + EXPECT_TRUE(trajectory.points.back().longitudinal_velocity_mps < 0.0); +} + +TEST(FreespacePlannerUtilsTest, testCreateStopTrajectory) +{ + geometry_msgs::msg::PoseStamped current_pose; + current_pose.pose.position.x = 1.0; + + auto stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(current_pose); + EXPECT_EQ(stop_traj.points.size(), 1ul); + if (!stop_traj.points.empty()) { + EXPECT_DOUBLE_EQ(stop_traj.points.front().pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(stop_traj.points.front().longitudinal_velocity_mps, 0.0); + } + + const auto trajectory = get_trajectory(0ul); + stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(trajectory); + EXPECT_EQ(stop_traj.points.size(), trajectory.points.size()); + if (!stop_traj.points.empty()) { + EXPECT_DOUBLE_EQ( + stop_traj.points.front().pose.position.x, trajectory.points.front().pose.position.x); + EXPECT_DOUBLE_EQ( + stop_traj.points.back().pose.position.x, trajectory.points.back().pose.position.x); + EXPECT_DOUBLE_EQ(stop_traj.points.front().longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(stop_traj.points.back().longitudinal_velocity_mps, 0.0); + } +} From 7cf1ffad44e480983ba24273dced8c720ab9ebc0 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 16 Oct 2024 10:22:31 +0900 Subject: [PATCH 26/27] test(freespace_planner): make unit tests for member functions (#9060) * refactor freespace planner Signed-off-by: mohammad alqudah * implement unit tests for freespace planner util functions Signed-off-by: mohammad alqudah * add freespace planner utils namespace Signed-off-by: mohammad alqudah * add function is_near_target to freespace planner utils Signed-off-by: mohammad alqudah * unit test for is_near_target function Signed-off-by: mohammad alqudah * implement unit tests for FreespacePlanner member functions Signed-off-by: mohammad alqudah * add docstring for functions Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../autoware_freespace_planner/CMakeLists.txt | 1 + .../freespace_planner_node.hpp | 40 +++- .../test/test_freespace_planner.cpp | 226 ++++++++++++++++++ .../test/test_freespace_planner_utils.cpp | 19 ++ 4 files changed, 281 insertions(+), 5 deletions(-) create mode 100644 planning/autoware_freespace_planner/test/test_freespace_planner.cpp diff --git a/planning/autoware_freespace_planner/CMakeLists.txt b/planning/autoware_freespace_planner/CMakeLists.txt index 12ec72d53d854..5c9e5cf15f9b0 100644 --- a/planning/autoware_freespace_planner/CMakeLists.txt +++ b/planning/autoware_freespace_planner/CMakeLists.txt @@ -18,6 +18,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_freespace_planner_node_interface.cpp test/test_freespace_planner_utils.cpp + test/test_freespace_planner.cpp ) target_link_libraries(test_${PROJECT_NAME} freespace_planner_node diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index b381f4dc1d0e7..55d7bf966b24f 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -64,6 +64,8 @@ #include #include +class TestFreespacePlanner; + namespace autoware::freespace_planner { using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; @@ -159,22 +161,50 @@ class FreespacePlannerNode : public rclcpp::Node void onRoute(const LaneletRoute::ConstSharedPtr msg); void onOdometry(const Odometry::ConstSharedPtr msg); + void onTimer(); void updateData(); + void reset(); + void planTrajectory(); + void initializePlanningAlgorithm(); bool isDataReady(); - void onTimer(); - - void reset(); + /** + * @brief Checks if a new trajectory planning is required. + * @details A new trajectory planning is required if: + * - Current trajectory points are empty, or + * - Current trajectory collides with an object, or + * - Ego deviates from current trajectory + * @return true if any of the conditions are met. + */ bool isPlanRequired(); - void planTrajectory(); + + /** + * @brief Sets the target index along the current trajectory points + * @details if Ego is stopped AND is near the current target index along the trajectory, + * then will get the next target index along the trajectory. + * If the new target index is the same as the current target index, then + * is_complete_ is set to true, and will publish is_completed_msg. + * Otherwise will update prev_target_index_ and target_index_, to continue + * following the trajectory. + */ void updateTargetIndex(); - void initializePlanningAlgorithm(); + /** + * @brief Checks if current trajectory is colliding with an object. + * @details Will check if an obstacle exists along the current trajectory, + * if there is no obstacle along the current trajectory, will reset obs_found_time_. + * If an obstacle exists and the variable obs_found_time_ is not initialized, + * will initialize with the current time. + * @return true if there is an obstacle along current trajectory, AND duration since + * obs_found_time_ exceeds the parameter th_obstacle_time_sec + */ bool checkCurrentTrajectoryCollision(); TransformStamped getTransform(const std::string & from, const std::string & to); std::unique_ptr logger_configure_; + + friend class ::TestFreespacePlanner; }; } // namespace autoware::freespace_planner diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner.cpp new file mode 100644 index 0000000000000..37ea52c07734b --- /dev/null +++ b/planning/autoware_freespace_planner/test/test_freespace_planner.cpp @@ -0,0 +1,226 @@ +// 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 "autoware/freespace_planner/freespace_planner_node.hpp" +#include "autoware/freespace_planner/utils.hpp" + +#include +#include +#include + +#include +#include + +#include +#include + +using autoware::freespace_planner::FreespacePlannerNode; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; +using nav_msgs::msg::Odometry; + +class TestFreespacePlanner : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + set_up_node(); + set_up_costmap(); + set_up_trajectory(); + } + + void set_up_node() + { + auto node_options = rclcpp::NodeOptions{}; + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto freespace_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_freespace_planner"); + node_options.arguments( + {"--ros-args", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + freespace_planner_dir + "/config/freespace_planner.param.yaml"}); + freespace_planner_ = std::make_shared(node_options); + } + + void set_up_costmap() + { + const size_t width = 80; + const size_t height = 80; + const double resolution = 0.5; + costmap_ = autoware::test_utils::makeCostMapMsg(width, height, resolution); + } + + void set_up_trajectory() + { + trajectory_.points.clear(); + + const double y = 0.5 * costmap_.info.height * costmap_.info.resolution; + + // forward trajectory for 20.0 m + double x = 5.0; + for (; x < 25.0 + std::numeric_limits::epsilon(); x += costmap_.info.resolution) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = 1.0; + trajectory_.points.push_back(point); + } + + // backward trajectory for 10.0 m + x = trajectory_.points.back().pose.position.x - costmap_.info.resolution; + for (; x > 15.0 - std::numeric_limits::epsilon(); x -= costmap_.info.resolution) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = -1.0; + trajectory_.points.push_back(point); + } + + // forward trajectory for 20.0 m + x = trajectory_.points.back().pose.position.x + costmap_.info.resolution; + for (; x < 35.0 + std::numeric_limits::epsilon(); x += costmap_.info.resolution) { + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + point.longitudinal_velocity_mps = 1.0; + trajectory_.points.push_back(point); + } + + reversing_indices = autoware::freespace_planner::utils::get_reversing_indices(trajectory_); + } + + [[nodiscard]] OccupancyGrid get_blocked_costmap() const + { + auto costmap = costmap_; + const auto block_y = 0.5 * costmap.info.height * costmap.info.resolution; + const auto block_x = 20.0; + const int index_x = std::round(block_x / costmap.info.resolution); + const int index_y = std::round(block_y / costmap.info.resolution); + const auto id = index_y * costmap.info.width + index_x; + costmap.data.at(id) = 100; + return costmap; + } + + bool test_is_plan_required( + const bool empty_traj = false, const bool colliding = false, const bool out_of_course = false) + { + freespace_planner_->trajectory_ = Trajectory(); + freespace_planner_->partial_trajectory_ = Trajectory(); + freespace_planner_->reversing_indices_.clear(); + freespace_planner_->obs_found_time_ = {}; + + freespace_planner_->occupancy_grid_ = std::make_shared(costmap_); + + if (empty_traj) { + return freespace_planner_->isPlanRequired(); + } + + freespace_planner_->trajectory_ = trajectory_; + freespace_planner_->reversing_indices_ = reversing_indices; + freespace_planner_->partial_trajectory_ = + autoware::freespace_planner::utils::get_partial_trajectory( + trajectory_, 0, reversing_indices.front()); + freespace_planner_->current_pose_.pose = trajectory_.points.front().pose; + + if (colliding) { + freespace_planner_->obs_found_time_ = freespace_planner_->get_clock()->now(); + freespace_planner_->occupancy_grid_ = std::make_shared(get_blocked_costmap()); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + if (out_of_course) { + freespace_planner_->current_pose_.pose.position.y += + (freespace_planner_->node_param_.th_course_out_distance_m + 0.1); + } + + return freespace_planner_->isPlanRequired(); + } + + std::pair test_update_target_index( + const bool stopped = false, const bool near_target = false) + { + freespace_planner_->trajectory_ = trajectory_; + freespace_planner_->reversing_indices_ = reversing_indices; + freespace_planner_->prev_target_index_ = 0; + freespace_planner_->target_index_ = autoware::freespace_planner::utils::get_next_target_index( + trajectory_.points.size(), reversing_indices, freespace_planner_->prev_target_index_); + freespace_planner_->partial_trajectory_ = + autoware::freespace_planner::utils::get_partial_trajectory( + trajectory_, freespace_planner_->prev_target_index_, freespace_planner_->target_index_); + + Odometry odom; + odom.pose.pose = trajectory_.points.front().pose; + odom.twist.twist.linear.x = 1.0; + + if (stopped) odom.twist.twist.linear.x = 0.0; + if (near_target) odom.pose.pose = trajectory_.points.at(freespace_planner_->target_index_).pose; + + freespace_planner_->odom_buffer_.clear(); + freespace_planner_->odom_ = std::make_shared(odom); + freespace_planner_->odom_buffer_.push_back(freespace_planner_->odom_); + freespace_planner_->current_pose_.pose = odom.pose.pose; + + freespace_planner_->updateTargetIndex(); + + return {freespace_planner_->prev_target_index_, freespace_planner_->target_index_}; + } + + void TearDown() override + { + freespace_planner_ = nullptr; + trajectory_ = Trajectory(); + costmap_ = OccupancyGrid(); + rclcpp::shutdown(); + } + + std::shared_ptr freespace_planner_; + OccupancyGrid costmap_; + Trajectory trajectory_; + std::vector reversing_indices; +}; + +TEST_F(TestFreespacePlanner, testIsPlanRequired) +{ + EXPECT_FALSE(test_is_plan_required()); + // test with empty current trajectory + EXPECT_TRUE(test_is_plan_required(true)); + // test with blocked trajectory + EXPECT_TRUE(test_is_plan_required(false, true)); + // test with deviation from trajectory + EXPECT_TRUE(test_is_plan_required(false, false, true)); +} + +TEST_F(TestFreespacePlanner, testUpdateTargetIndex) +{ + size_t prev_target_index, target_index; + std::tie(prev_target_index, target_index) = test_update_target_index(); + EXPECT_EQ(prev_target_index, 0ul); + EXPECT_EQ(target_index, reversing_indices.front()); + + std::tie(prev_target_index, target_index) = test_update_target_index(true); + EXPECT_EQ(prev_target_index, 0ul); + EXPECT_EQ(target_index, reversing_indices.front()); + + std::tie(prev_target_index, target_index) = test_update_target_index(false, true); + EXPECT_EQ(prev_target_index, 0ul); + EXPECT_EQ(target_index, reversing_indices.front()); + + std::tie(prev_target_index, target_index) = test_update_target_index(true, true); + EXPECT_EQ(prev_target_index, reversing_indices.front()); + EXPECT_EQ(target_index, *std::next(reversing_indices.begin())); +} diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp index bccff30e11583..071f7e2cfcaa0 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp @@ -148,6 +148,25 @@ TEST(FreespacePlannerUtilsTest, testIsStopped) autoware::freespace_planner::utils::is_stopped(odometry_buffer, th_stopped_velocity_mps)); } +TEST(FreespacePlannerUtilsTest, testIsNearTarget) +{ + const auto trajectory = get_trajectory(0ul); + const auto target_pose = trajectory.points.back().pose; + + auto current_pose = target_pose; + current_pose.position.x -= 1.0; + current_pose.position.y += 1.0; + + const double th_distance_m = 0.5; + + EXPECT_FALSE( + autoware::freespace_planner::utils::is_near_target(target_pose, current_pose, th_distance_m)); + + current_pose.position.x += 0.6; + EXPECT_TRUE( + autoware::freespace_planner::utils::is_near_target(target_pose, current_pose, th_distance_m)); +} + TEST(FreespacePlannerUtilsTest, testGetReversingIndices) { auto trajectory = get_trajectory(0ul); From a3c7ede30310891996a7249fc60b3405fc913567 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 16 Oct 2024 10:40:31 +0900 Subject: [PATCH 27/27] test(bpp_common): add test for objects filtering except for lanelet using functions (#9049) * add test for objects filtering except for lanelet using functions Signed-off-by: Go Sakayori * remove unnecessary include file Signed-off-by: Go Sakayori * add doxygen Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- .../CMakeLists.txt | 1 + .../path_safety_checker/objects_filtering.hpp | 32 +- .../path_safety_checker/objects_filtering.cpp | 2 + .../test/test_objects_filtering.cpp | 382 ++++++++++++++++++ 4 files changed, 416 insertions(+), 1 deletion(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index e24abb51de6fc..d3f76ed904244 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -60,6 +60,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check test/test_safety_check.cpp + test/test_objects_filtering.cpp ) target_link_libraries(test_${PROJECT_NAME}_safety_check diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 3d7532ba5b244..d9873975dabe1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -34,14 +34,44 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter using autoware_perception_msgs::msg::PredictedObject; using tier4_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters object based on velocity. + * + * @param object The predicted object to filter. + * @param velocity_threshold Lower bound + * @param max_velocity Upper bound + * @return Returns true when the object is within a certain velocity range. + */ bool velocity_filter( const PredictedObject & object, double velocity_threshold, double max_velocity); +/** + * @brief Filters object based on position. + * + * @param object The predicted object to filter. + * @param path_points Ego path + * @param current_pose Ego current pose + * @param forward_distance Upper bound for relative distance + * @param backward_distance Lower bound for relative distance + * @return Returns true when the object is within a certain distance. + */ bool position_filter( - PredictedObject & object, const std::vector & path_points, + const PredictedObject & object, const std::vector & path_points, const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Filters object by searching within a radius. + * + * @param object The predicted object to filter. + * @param reference_point Reference point + * @param search_radius Search radius + * @return Returns true when the object is within radius. + */ +bool is_within_circle( + const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point, + const double search_radius); + } // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index f183f5160cea8..80936b655cc48 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -39,6 +39,8 @@ bool position_filter( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance) { + if (path_points.empty()) return false; + const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp new file mode 100644 index 0000000000000..a2058db51c9ea --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -0,0 +1,382 @@ +// 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 "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include + +#include +#include + +using PredictedObject = autoware_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; +using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; +using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; +using TwistWithCovariance = geometry_msgs::msg::TwistWithCovariance; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; + +using autoware::test_utils::createPose; +using autoware::test_utils::generateTrajectory; +using autoware::universe_utils::createPoint; + +constexpr double epsilon = 1e-6; + +std::vector trajectory_to_path_with_lane_id(const Trajectory & trajectory) +{ + std::vector path_with_lane_id; + PathPointWithLaneId path_point_with_lane_id; + for (const auto & point : trajectory.points) { + path_point_with_lane_id.point.pose = point.pose; + path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps; + path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps; + path_with_lane_id.push_back(path_point_with_lane_id); + } + return path_with_lane_id; +} + +PredictedPath trajectory_to_predicted_path(const Trajectory & trajectory) +{ + PredictedPath path; + geometry_msgs::msg::Pose pose; + for (const auto & point : trajectory.points) { + pose = point.pose; + path.path.push_back(pose); + } + return path; +} + +TEST(BehaviorPathPlanningObjectsFiltering, velocity_filter) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; + + PredictedObject predicted_obj; + predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 4.0; + predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 3.0; + + EXPECT_TRUE(velocity_filter(predicted_obj, 4.0, 10.0)); + EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 10.0)); + EXPECT_FALSE(velocity_filter(predicted_obj, 2.0, 4.9)); + EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 2.0)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, position_filter) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filter::position_filter; + + auto current_pos = createPoint(0.0, 0.0, 0.0); + PredictedObject object; + object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0); + auto straight_trajectory = generateTrajectory(20, 1.0); + double forward_distance = 20.0; + double backward_distance = 1.0; + + std::vector empty_path; + auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); + auto curved_path = + trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0, 0.0, 0.0, 0.01)); + + EXPECT_TRUE( + position_filter(object, straight_path, current_pos, forward_distance, -backward_distance)); + EXPECT_FALSE( + position_filter(object, empty_path, current_pos, forward_distance, -backward_distance)); + EXPECT_TRUE( + position_filter(object, curved_path, current_pos, forward_distance, -backward_distance)); + + forward_distance = 2.0; + EXPECT_FALSE( + position_filter(object, straight_path, current_pos, forward_distance, -backward_distance)); + EXPECT_FALSE( + position_filter(object, curved_path, current_pos, forward_distance, -backward_distance)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, is_within_circle) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filter::is_within_circle; + + auto object_pos = createPoint(0.0, 0.0, 0.0); + auto ref_point = createPoint(0.0, 0.0, 0.0); + double search_radius = 1.0; + + EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); + + object_pos.x = 2.0; + object_pos.x = 2.0; + + EXPECT_FALSE(is_within_circle(object_pos, ref_point, search_radius)); + + ref_point.x = 2.5; + ref_point.y = 2.4; + object_pos.x = 3.0; + object_pos.y = 3.0; + + EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByVelocity) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByVelocity; + + PredictedObjects objects; + PredictedObject slow_obj; + PredictedObject fast_obj; + double vel_thr = 5.0; + + slow_obj.object_id = autoware::universe_utils::generateUUID(); + slow_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; + slow_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; + objects.objects.push_back(slow_obj); + + fast_obj.object_id = autoware::universe_utils::generateUUID(); + fast_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 10.0; + fast_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; + objects.objects.push_back(fast_obj); + + auto filtered_obj = filterObjectsByVelocity(objects, vel_thr, false); + ASSERT_FALSE(filtered_obj.objects.empty()); + EXPECT_EQ(filtered_obj.objects.front().object_id, slow_obj.object_id); + + filtered_obj = filterObjectsByVelocity(objects, vel_thr, true); + ASSERT_FALSE(filtered_obj.objects.empty()); + EXPECT_EQ(filtered_obj.objects.front().object_id, fast_obj.object_id); + + vel_thr = 0.0; + filtered_obj = filterObjectsByVelocity(objects, vel_thr, false); + EXPECT_TRUE(filtered_obj.objects.empty()); + + filtered_obj = filterObjectsByVelocity(objects, vel_thr, true); + EXPECT_EQ(filtered_obj.objects.size(), 2); +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByPosition; + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsWithinRadius; + + auto current_pos = createPoint(0.0, 0.0, 0.0); + auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); + double forward_distance = 10.0; + double backward_distance = 1.0; + double search_radius = 10.0; + + PredictedObjects objects; + PredictedObject far_obj; + PredictedObject near_obj; + + near_obj.object_id = autoware::universe_utils::generateUUID(); + near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0); + objects.objects.push_back(near_obj); + auto target_uuid = near_obj.object_id; + + far_obj.object_id = autoware::universe_utils::generateUUID(); + far_obj.kinematics.initial_pose_with_covariance.pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); + objects.objects.push_back(far_obj); + + filterObjectsByPosition(objects, straight_path, current_pos, forward_distance, backward_distance); + + ASSERT_FALSE(objects.objects.empty()); + EXPECT_EQ(objects.objects.front().object_id, target_uuid); + + objects.objects.clear(); + objects.objects.push_back(far_obj); + objects.objects.push_back(near_obj); + + filterObjectsWithinRadius(objects, current_pos, search_radius); + ASSERT_FALSE(objects.objects.empty()); + EXPECT_EQ(objects.objects.front().object_id, target_uuid); +} + +TEST(BehaviorPathPlanningObjectsFiltering, createPredictedPath) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; + + std::shared_ptr< + autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams> + param = std::make_shared< + autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams>(); + + param->min_velocity = -1.0; + param->acceleration = 0.1; + param->max_velocity = 5.0; + param->time_horizon_for_front_object = 2.0; + param->time_horizon_for_rear_object = 1.0; + param->time_resolution = 0.5; + param->delay_until_departure = 0.1; + + auto vehicle_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + std::vector empty_path; + auto predicted_path = createPredictedPath(param, empty_path, vehicle_pose, 0.0, 0, false, false); + EXPECT_TRUE(predicted_path.empty()); + + size_t i = 0; + auto straight_path = trajectory_to_path_with_lane_id(generateTrajectory(20, 1.0)); + predicted_path = createPredictedPath(param, straight_path, vehicle_pose, 0.0, 0, true, false); + + ASSERT_EQ(predicted_path.size(), 4); + for (const auto & point : predicted_path) { + auto time = + std::max(0.0, param->time_resolution * static_cast(i) - param->delay_until_departure); + EXPECT_NEAR(point.time, i * param->time_resolution, epsilon); + EXPECT_NEAR(point.pose.position.x, std::pow(time, 2.0) * param->acceleration * 0.5, epsilon); + EXPECT_NEAR(point.velocity, time * param->acceleration, epsilon); + i++; + } + + i = 0; + predicted_path = createPredictedPath(param, straight_path, vehicle_pose, 0.0, 0, false, false); + + ASSERT_EQ(predicted_path.size(), 2); + for (const auto & point : predicted_path) { + auto time = + std::max(0.0, param->time_resolution * static_cast(i) - param->delay_until_departure); + EXPECT_NEAR(point.time, i * param->time_resolution, epsilon); + EXPECT_NEAR(point.pose.position.x, std::pow(time, 2.0) * param->acceleration * 0.5, epsilon); + EXPECT_NEAR(point.velocity, time * param->acceleration, epsilon); + i++; + } +} + +TEST(BehaviorPathPlanningObjectsFiltering, transform) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::transform; + auto velocity = autoware::universe_utils::createVector3(2.0, 0.0, 0.0); + auto angular = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); + + PredictedObject obj; + obj.object_id = autoware::universe_utils::generateUUID(); + obj.kinematics.initial_pose_with_covariance.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); + obj.kinematics.initial_twist_with_covariance.twist = + autoware::universe_utils::createTwist(velocity, angular); + autoware_perception_msgs::msg::PredictedPath predicted_path; + auto straight_path = trajectory_to_predicted_path(generateTrajectory(5, 1.0)); + straight_path.confidence = 0.6; + straight_path.time_step.sec = 1.0; + obj.kinematics.predicted_paths.push_back(straight_path); + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 1.0; + obj.shape.dimensions.z = 1.0; + + double safety_check_time_horizon = 2.0; + double safety_check_time_resolution = 0.5; + + auto extended_obj = transform(obj, safety_check_time_horizon, safety_check_time_resolution); + EXPECT_NEAR( + extended_obj.predicted_paths.front().confidence, + obj.kinematics.predicted_paths.front().confidence, epsilon); + ASSERT_FALSE(extended_obj.predicted_paths.empty()); + EXPECT_EQ(extended_obj.predicted_paths.front().path.size(), 5); + + size_t i = 0; + for (const auto & point : extended_obj.predicted_paths.front().path) { + EXPECT_NEAR(point.pose.position.x, i * 0.5, epsilon); + EXPECT_NEAR(point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(point.velocity, velocity.x, epsilon); + ASSERT_EQ(point.poly.outer().size(), 5); + + EXPECT_NEAR( + point.poly.outer().at(0).x(), point.pose.position.x + 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(0).y(), point.pose.position.y + 0.5 * obj.shape.dimensions.y, epsilon); + + EXPECT_NEAR( + point.poly.outer().at(1).x(), point.pose.position.x + 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(1).y(), point.pose.position.y - 0.5 * obj.shape.dimensions.y, epsilon); + + EXPECT_NEAR( + point.poly.outer().at(2).x(), point.pose.position.x - 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(2).y(), point.pose.position.y - 0.5 * obj.shape.dimensions.y, epsilon); + + EXPECT_NEAR( + point.poly.outer().at(3).x(), point.pose.position.x - 0.5 * obj.shape.dimensions.x, epsilon); + EXPECT_NEAR( + point.poly.outer().at(3).y(), point.pose.position.y + 0.5 * obj.shape.dimensions.y, epsilon); + i++; + } +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByClass) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByClass; + + PredictedObjects objects; + PredictedObject car; + PredictedObject truck; + PredictedObject pedestrian; + ObjectClassification classification; + autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck types_to_check; + + car.object_id = autoware::universe_utils::generateUUID(); + classification.label = ObjectClassification::Type::CAR; + classification.probability = 1.0; + car.classification.push_back(classification); + objects.objects.push_back(car); + + truck.object_id = autoware::universe_utils::generateUUID(); + classification.label = ObjectClassification::Type::TRUCK; + classification.probability = 1.0; + truck.classification.push_back(classification); + objects.objects.push_back(truck); + + pedestrian.object_id = autoware::universe_utils::generateUUID(); + classification.label = ObjectClassification::Type::PEDESTRIAN; + classification.probability = 1.0; + pedestrian.classification.push_back(classification); + objects.objects.push_back(pedestrian); + + filterObjectsByClass(objects, types_to_check); + EXPECT_EQ(objects.objects.size(), 3); + + types_to_check.check_pedestrian = false; + filterObjectsByClass(objects, types_to_check); + EXPECT_EQ(objects.objects.size(), 2); + + types_to_check.check_car = false; + types_to_check.check_truck = false; + filterObjectsByClass(objects, types_to_check); + EXPECT_TRUE(objects.objects.empty()); +} + +TEST(BehaviorPathPlanningObjectsFiltering, isTargetObjectType) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::isTargetObjectType; + + autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck types_to_check; + PredictedObject obj; + ObjectClassification classification; + classification.label = ObjectClassification::Type::CAR; + classification.probability = 0.6; + obj.classification.push_back(classification); + classification.label = ObjectClassification::Type::TRUCK; + classification.probability = 0.4; + obj.classification.push_back(classification); + + EXPECT_TRUE(isTargetObjectType(obj, types_to_check)); + types_to_check.check_car = false; + EXPECT_FALSE(isTargetObjectType(obj, types_to_check)); + + obj.classification.front().probability = 0.4; + obj.classification.at(1).probability = 0.6; + EXPECT_TRUE(isTargetObjectType(obj, types_to_check)); + + obj.classification.at(1).label = ObjectClassification::Type::PEDESTRIAN; + EXPECT_TRUE(isTargetObjectType(obj, types_to_check)); +}