diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 256b617116432..a6cdc58f03e94 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -205,7 +205,7 @@ planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp +planning/static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp diff --git a/build_depends.repos b/build_depends.repos index f7b3f12484015..5a12a67dbd346 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -41,3 +41,8 @@ repositories: type: git url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git version: main + #vehicle + vehicle/sample_vehicle_launch: + type: git + url: https://github.com/autowarefoundation/sample_vehicle_launch.git + version: main diff --git a/common/global_parameter_loader/CMakeLists.txt b/common/global_parameter_loader/CMakeLists.txt index e67ae1a5c06fc..eaca44c515452 100644 --- a/common/global_parameter_loader/CMakeLists.txt +++ b/common/global_parameter_loader/CMakeLists.txt @@ -4,6 +4,11 @@ project(global_parameter_loader) find_package(autoware_cmake REQUIRED) autoware_package() +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_global_params_launch ${test_files}) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/common/global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml index 4c2568b9aec98..78d08e4038250 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -10,8 +10,10 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + sample_vehicle_description + vehicle_info_util + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/global_parameter_loader/test/test_global_params_launch.cpp b/common/global_parameter_loader/test/test_global_params_launch.cpp new file mode 100644 index 0000000000000..2b33a695253ff --- /dev/null +++ b/common/global_parameter_loader/test/test_global_params_launch.cpp @@ -0,0 +1,44 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +TEST(TestLaunchFile, test_launch_file) +{ + // Define the path of Python launch file + std::string global_params_launch_path = "global_params.launch.py"; + + // Define the parameters you want to pass to the launch file + std::string use_sim_time_param = "false"; + std::string vehicle_model_param = "sample_vehicle"; + // Construct the command to run the Python launch script with parameters + std::string command = "ros2 launch global_parameter_loader " + global_params_launch_path + + " use_sim_time:=" + use_sim_time_param + + " vehicle_model:=" + vehicle_model_param; + + // Use the system() function to execute the command + int result = std::system(command.c_str()); + // Check the result of running the launch file + EXPECT_EQ(result, 0); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp index 5e8b6a3d49890..e9b924023f62e 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/matching.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/matching.hpp @@ -29,6 +29,8 @@ namespace object_recognition_utils { using tier4_autoware_utils::Polygon2d; +// minimum area to avoid division by zero +static const double MIN_AREA = 1e-6; inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon2d & target_polygon) { @@ -66,10 +68,12 @@ template double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01) { const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); - if (intersection_area == 0.0) return 0.0; + if (intersection_area < MIN_AREA) return 0.0; const double union_area = getUnionArea(source_polygon, target_polygon); const double iou = @@ -81,7 +85,9 @@ template double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) { const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); const double union_area = getUnionArea(source_polygon, target_polygon); @@ -95,11 +101,13 @@ template double get2dPrecision(const T1 source_object, const T2 target_object) { const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + const double source_area = boost::geometry::area(source_polygon); + if (source_area < MIN_AREA) return 0.0; const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); - if (intersection_area == 0.0) return 0.0; - const double source_area = boost::geometry::area(source_polygon); + if (intersection_area < MIN_AREA) return 0.0; return std::min(1.0, intersection_area / source_area); } @@ -108,11 +116,13 @@ template double get2dRecall(const T1 source_object, const T2 target_object) { const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object); + const double target_area = boost::geometry::area(target_polygon); + if (target_area < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); - if (intersection_area == 0.0) return 0.0; - const double target_area = boost::geometry::area(target_polygon); + if (intersection_area < MIN_AREA) return 0.0; return std::min(1.0, intersection_area / target_area); } diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp new file mode 100644 index 0000000000000..f8d5baaf02777 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ + +#include + +#include + +namespace tier4_autoware_utils +{ + +template +class InterProcessPollingSubscriber +{ +private: + typename rclcpp::Subscription::SharedPtr subscriber_; + std::optional data_; + +public: + explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name) + { + auto noexec_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + + subscriber_ = node->create_subscription( + topic_name, rclcpp::QoS{1}, + [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, + noexec_subscription_options); + }; + bool updateLatestData() + { + rclcpp::MessageInfo message_info; + T tmp; + // The queue size (QoS) must be 1 to get the last message data. + if (subscriber_->take(tmp, message_info)) { + data_ = tmp; + } + return data_.has_value(); + }; + const T & getData() const + { + if (!data_.has_value()) { + throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber"); + } + return data_.value(); + }; +}; + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 0cf2548d69746..bba069442bee7 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -537,6 +537,10 @@ void PredictedPathCheckerNode::filterObstacles( const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, const bool use_prediction, PredictedObjects & filtered_objects) { + if (traj.size() < 2) { + RCLCPP_ERROR(rclcpp::get_logger("predicted_path_checker"), "Trajectory size is less than 2."); + return; + } filtered_objects.header.frame_id = object_ptr_.get()->header.frame_id; filtered_objects.header.stamp = this->now(); diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index c0e2a516cf948..7c2179239221e 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -11,6 +11,7 @@ This module allows for the evaluation of how accurately perception results are g - Calculates lateral deviation between the predicted path and the actual traveled trajectory. - Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition. - Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition. +- Calculates yaw rate based on the yaw of the object received in the previous cycle to evaluate the stability of the yaw rate recognition. ## Inputs / Outputs diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index 3ac4c1db9efbd..0831d580248d3 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -63,8 +63,8 @@ MarkerArray createPoseMarkerArray( const float & b); MarkerArray createPosesMarkerArray( - const std::vector poses, std::string && ns, const float & r, const float & g, - const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, + const std::vector poses, std::string && ns, const int32_t & first_id, const float & r, + const float & g, const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, const float & z_scale = 0.2); std_msgs::msg::ColorRGBA createColorFromString(const std::string & str); @@ -75,7 +75,7 @@ MarkerArray createObjectPolygonMarkerArray( MarkerArray createDeviationLines( const std::vector poses1, const std::vector poses2, const std::string & ns, - const float r, const float g, const float b); + const int32_t & first_id, const float r, const float g, const float b); } // namespace marker_utils diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 976b10a2c73f9..46ea68c991090 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -11,7 +11,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] - stopped_velocity_threshold: 0.3 + stopped_velocity_threshold: 1.0 target_object: car: diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 60e02c8f24c39..4b882bb8b2e68 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -37,20 +37,25 @@ std::optional MetricsCalculator::calculate(const Metric & metric) return {}; } const auto target_stamp_objects = getObjectsByStamp(target_stamp); - const auto class_objects_map = separateObjectsByClass(target_stamp_objects); - // filter stopped objects + // extract moving objects + const auto moving_objects = filterObjectsByVelocity( + target_stamp_objects, parameters_->stopped_velocity_threshold, + /*remove_above_threshold=*/false); + const auto class_moving_objects_map = separateObjectsByClass(moving_objects); + + // extract stopped objects const auto stopped_objects = filterObjectsByVelocity(target_stamp_objects, parameters_->stopped_velocity_threshold); const auto class_stopped_objects_map = separateObjectsByClass(stopped_objects); switch (metric) { case Metric::lateral_deviation: - return calcLateralDeviationMetrics(class_objects_map); + return calcLateralDeviationMetrics(class_moving_objects_map); case Metric::yaw_deviation: - return calcYawDeviationMetrics(class_objects_map); + return calcYawDeviationMetrics(class_moving_objects_map); case Metric::predicted_path_deviation: - return calcPredictedPathDeviationMetrics(class_objects_map); + return calcPredictedPathDeviationMetrics(class_moving_objects_map); case Metric::yaw_rate: return calcYawRateMetrics(class_stopped_objects_map); default: @@ -372,16 +377,21 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas } const auto [previous_stamp, previous_object] = previous_object_with_stamp_opt.value(); - const auto time_diff = (stamp - previous_stamp).seconds(); + const double time_diff = (stamp - previous_stamp).seconds(); if (time_diff < 0.01) { continue; } - const auto current_yaw = + const double current_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); - const auto previous_yaw = + const double previous_yaw = tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); - const auto yaw_rate = - std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw) / time_diff); + const double yaw_diff = + std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw)); + // if yaw_diff is close to PI, reversal of orientation is likely occurring, so ignore it + if (std::abs(M_PI - yaw_diff) < 0.1) { + continue; + } + const auto yaw_rate = yaw_diff / time_diff; stat.add(yaw_rate); } metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat; @@ -443,7 +453,34 @@ void MetricsCalculator::updateHistoryPath() for (const auto & [uuid, stamp_and_objects] : object_map_) { std::vector history_path; - for (const auto & [stamp, object] : stamp_and_objects) { + for (auto it = stamp_and_objects.begin(); it != stamp_and_objects.end(); ++it) { + const auto & [stamp, object] = *it; + + // skip if the object is stopped + // calculate velocity from previous object + if (it != stamp_and_objects.begin()) { + const auto & [prev_stamp, prev_object] = *std::prev(it); + const double time_diff = (stamp - prev_stamp).seconds(); + if (time_diff < 0.01) { + continue; + } + const auto current_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto prev_pose = prev_object.kinematics.initial_pose_with_covariance.pose; + const auto velocity = + tier4_autoware_utils::calcDistance2d(current_pose.position, prev_pose.position) / + time_diff; + if (velocity < parameters_->stopped_velocity_threshold) { + continue; + } + } + if ( + std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y) < + parameters_->stopped_velocity_threshold) { + continue; + } + history_path.push_back(object.kinematics.initial_pose_with_covariance.pose); } @@ -507,27 +544,49 @@ std::vector MetricsCalculator::averageFilterPath( average_pose.position = path.at(i).position; } + // skip if the points are too close + if ( + filtered_path.size() > 0 && + calcDistance2d(filtered_path.back().position, average_pose.position) < 0.5) { + continue; + } + + // skip if the difference between the current orientation and the azimuth angle is large + if (i > 0) { + const double azimuth = calcAzimuthAngle(path.at(i - 1).position, path.at(i).position); + const double yaw = tf2::getYaw(path.at(i).orientation); + if (tier4_autoware_utils::normalizeRadian(yaw - azimuth) > M_PI_2) { + continue; + } + } + // Placeholder for orientation to ensure structure integrity average_pose.orientation = path.at(i).orientation; filtered_path.push_back(average_pose); } - // Calculate yaw and convert to quaternion after averaging positions - for (size_t i = 0; i < filtered_path.size(); ++i) { - Pose & p = filtered_path[i]; - - // if the current pose is too close to the previous one, use the previous orientation - if (i > 0) { - const Pose & p_prev = filtered_path[i - 1]; - if (calcDistance2d(p_prev.position, p.position) < 0.1) { - p.orientation = p_prev.orientation; + // delete pose if the difference between the azimuth angle of the previous and next poses is large + if (filtered_path.size() > 2) { + auto it = filtered_path.begin() + 2; + while (it != filtered_path.end()) { + const double azimuth_to_prev = calcAzimuthAngle((it - 2)->position, (it - 1)->position); + const double azimuth_to_current = calcAzimuthAngle((it - 1)->position, it->position); + if ( + std::abs(tier4_autoware_utils::normalizeRadian(azimuth_to_prev - azimuth_to_current)) > + M_PI_2) { + it = filtered_path.erase(it); continue; } + ++it; } + } + // Calculate yaw and convert to quaternion after averaging positions + for (size_t i = 0; i < filtered_path.size(); ++i) { + Pose & p = filtered_path[i]; if (i < filtered_path.size() - 1) { - const double yaw = calcAzimuthAngle(p.position, filtered_path[i + 1].position); - filtered_path[i].orientation = createQuaternionFromYaw(yaw); + const double azimuth_to_next = calcAzimuthAngle(p.position, filtered_path[i + 1].position); + filtered_path[i].orientation = createQuaternionFromYaw(azimuth_to_next); } else if (filtered_path.size() > 1) { // For the last point, use the orientation of the second-to-last point p.orientation = filtered_path[i - 1].orientation; diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 8820ad16fd8d5..ceb304894ad8c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -132,50 +132,74 @@ void PerceptionOnlineEvaluatorNode::publishDebugMarker() tier4_autoware_utils::appendMarkerArray(added, &marker); }; - const auto history_path_map = metrics_calculator_.getHistoryPathMap(); const auto & p = parameters_->debug_marker_parameters; - for (const auto & [uuid, history_path] : history_path_map) { - { - const auto c = createColorFromString(uuid + "_raw"); - if (p.show_history_path) { - add(createPointsMarkerArray(history_path.first, "history_path_" + uuid, 0, c.r, c.g, c.b)); + // visualize history path + { + const auto history_path_map = metrics_calculator_.getHistoryPathMap(); + int32_t history_path_first_id = 0; + int32_t smoothed_history_path_first_id = 0; + size_t i = 0; + for (const auto & [uuid, history_path] : history_path_map) { + { + const auto c = createColorFromString(uuid + "_raw"); + if (p.show_history_path) { + add(createPointsMarkerArray(history_path.first, "history_path", i, c.r, c.g, c.b)); + } + if (p.show_history_path_arrows) { + add(createPosesMarkerArray( + history_path.first, "history_path_arrows", history_path_first_id, c.r, c.g, c.b, 0.1, + 0.05, 0.05)); + history_path_first_id += history_path.first.size(); + } } - if (p.show_history_path_arrows) { - add(createPosesMarkerArray( - history_path.first, "history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, 0.05)); + { + const auto c = createColorFromString(uuid); + if (p.show_smoothed_history_path) { + add(createPointsMarkerArray( + history_path.second, "smoothed_history_path", i, c.r, c.g, c.b)); + } + if (p.show_smoothed_history_path_arrows) { + add(createPosesMarkerArray( + history_path.second, "smoothed_history_path_arrows", smoothed_history_path_first_id, + c.r, c.g, c.b, 0.1, 0.05, 0.05)); + smoothed_history_path_first_id += history_path.second.size(); + } } + i++; } - { + } + + // visualize predicted path of past objects + { + int32_t predicted_path_first_id = 0; + int32_t history_path_first_id = 0; + int32_t deviation_lines_first_id = 0; + size_t i = 0; + const auto object_data_map = metrics_calculator_.getDebugObjectData(); + for (const auto & [uuid, object_data] : object_data_map) { const auto c = createColorFromString(uuid); - if (p.show_smoothed_history_path) { - add(createPointsMarkerArray( - history_path.second, "smoothed_history_path_" + uuid, 0, c.r, c.g, c.b)); + const auto predicted_path = object_data.getPredictedPath(); + const auto history_path = object_data.getHistoryPath(); + if (p.show_predicted_path) { + add(createPosesMarkerArray( + predicted_path, "predicted_path", predicted_path_first_id, 0, 0, 1)); + predicted_path_first_id += predicted_path.size(); } - if (p.show_smoothed_history_path_arrows) { + if (p.show_predicted_path_gt) { add(createPosesMarkerArray( - history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, - 0.05)); + history_path, "predicted_path_gt", history_path_first_id, 1, 0, 0)); + history_path_first_id += history_path.size(); } - } - } - const auto object_data_map = metrics_calculator_.getDebugObjectData(); - for (const auto & [uuid, object_data] : object_data_map) { - const auto c = createColorFromString(uuid); - const auto predicted_path = object_data.getPredictedPath(); - const auto history_path = object_data.getHistoryPath(); - if (p.show_predicted_path) { - add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 1)); - } - if (p.show_predicted_path_gt) { - add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 1, 0, 0)); - } - if (p.show_deviation_lines) { - add(createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 1, 1, 1)); - } - if (p.show_object_polygon) { - add(createObjectPolygonMarkerArray( - object_data.object, "object_polygon_" + uuid, 0, c.r, c.g, c.b)); + if (p.show_deviation_lines) { + add(createDeviationLines( + predicted_path, history_path, "deviation_lines", deviation_lines_first_id, 1, 1, 1)); + deviation_lines_first_id += predicted_path.size(); + } + if (p.show_object_polygon) { + add(createObjectPolygonMarkerArray(object_data.object, "object_polygon", i, c.r, c.g, c.b)); + } + i++; } } @@ -190,6 +214,7 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame auto & p = parameters_; updateParam(parameters, "smoothing_window_size", p->smoothing_window_size); + updateParam(parameters, "stopped_velocity_threshold", p->stopped_velocity_threshold); // update metrics { diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 75af92e83dcd8..a29e1a468e983 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -111,14 +111,14 @@ MarkerArray createPointsMarkerArray( MarkerArray createDeviationLines( const std::vector poses1, const std::vector poses2, const std::string & ns, - const float r, const float g, const float b) + const int32_t & first_id, const float r, const float g, const float b) { MarkerArray msg; const size_t max_idx = std::min(poses1.size(), poses2.size()); for (size_t i = 0; i < max_idx; ++i) { auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::LINE_STRIP, + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, first_id + i, Marker::LINE_STRIP, createMarkerScale(0.005, 0.0, 0.0), createMarkerColor(r, g, b, 0.5)); marker.points.push_back(poses1.at(i).position); marker.points.push_back(poses2.at(i).position); @@ -144,14 +144,15 @@ MarkerArray createPoseMarkerArray( } MarkerArray createPosesMarkerArray( - const std::vector poses, std::string && ns, const float & r, const float & g, - const float & b, const float & x_scale, const float & y_scale, const float & z_scale) + const std::vector poses, std::string && ns, const int32_t & first_id, const float & r, + const float & g, const float & b, const float & x_scale, const float & y_scale, + const float & z_scale) { MarkerArray msg; for (size_t i = 0; i < poses.size(); ++i) { Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, first_id + i, Marker::ARROW, createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5)); marker.pose = poses.at(i); msg.markers.push_back(marker); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 2975a1b4d6df8..dabd17b9db46a 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -115,7 +115,7 @@ class EvalTest : public ::testing::Test PredictedObject makePredictedObject( const std::vector> & predicted_path, - const uint8_t label = ObjectClassification::CAR) + const uint8_t label = ObjectClassification::CAR, const double velocity = 2.0) { PredictedObject object; object.object_id = uuid_; @@ -133,6 +133,10 @@ class EvalTest : public ::testing::Test object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0.0; object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1.0; + object.kinematics.initial_twist_with_covariance.twist.linear.x = velocity; + object.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; + object.kinematics.initial_twist_with_covariance.twist.linear.z = 0.0; + autoware_auto_perception_msgs::msg::PredictedPath path; for (size_t i = 0; i < predicted_path.size(); ++i) { geometry_msgs::msg::Pose pose; @@ -155,34 +159,35 @@ class EvalTest : public ::testing::Test PredictedObjects makePredictedObjects( const std::vector> & predicted_path, - const uint8_t label = ObjectClassification::CAR) + const uint8_t label = ObjectClassification::CAR, const double velocity = 2.0) { PredictedObjects objects; - objects.objects.push_back(makePredictedObject(predicted_path, label)); + objects.objects.push_back(makePredictedObject(predicted_path, label, velocity)); objects.header.stamp = rclcpp::Time(0); return objects; } PredictedObjects makeStraightPredictedObjects( - const double time, const uint8_t label = ObjectClassification::CAR) + const double time, const uint8_t label = ObjectClassification::CAR, const double velocity = 2.0) { std::vector> predicted_path; for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { - predicted_path.push_back({velocity_ * (time + i * time_step_), 0.0}); + predicted_path.push_back({velocity * (time + i * time_step_), 0.0}); } - auto objects = makePredictedObjects(predicted_path, label); + auto objects = makePredictedObjects(predicted_path, label, velocity); objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); return objects; } PredictedObjects makeDeviatedStraightPredictedObjects( - const double time, const double deviation, const uint8_t label = ObjectClassification::CAR) + const double time, const double deviation, const uint8_t label = ObjectClassification::CAR, + const double velocity = 2.0) { std::vector> predicted_path; for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { - predicted_path.push_back({velocity_ * (time + i * time_step_), deviation}); + predicted_path.push_back({velocity * (time + i * time_step_), deviation}); } - auto objects = makePredictedObjects(predicted_path, label); + auto objects = makePredictedObjects(predicted_path, label, velocity); objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); return objects; } @@ -249,7 +254,6 @@ class EvalTest : public ::testing::Test double time_delay_ = 5.0; double time_step_ = 0.5; double time_horizon_ = 10.0; - double velocity_ = 2.0; unique_identifier_msgs::msg::UUID uuid_; }; @@ -589,11 +593,12 @@ TEST_F(EvalTest, testYawRate_0) setTargetMetric("yaw_rate_CAR"); for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = makeStraightPredictedObjects(time); + const auto objects = makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0); publishObjects(objects); } - const auto last_objects = makeStraightPredictedObjects(time_delay_ + time_step_); + const auto last_objects = + makeStraightPredictedObjects(time_delay_ + time_step_, ObjectClassification::CAR, 0.0); EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); } @@ -605,12 +610,14 @@ TEST_F(EvalTest, testYawRate_01) const double yaw_rate = 0.1; for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); publishObjects(objects); } for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); } } @@ -623,12 +630,14 @@ TEST_F(EvalTest, testYawRate_minus_01) const double yaw_rate = 0.1; for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); publishObjects(objects); } for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); } } @@ -641,12 +650,14 @@ TEST_F(EvalTest, testYawRate_1) const double yaw_rate = 1.0; for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); publishObjects(objects); } for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); } } @@ -659,12 +670,14 @@ TEST_F(EvalTest, testYawRate_minus_1) const double yaw_rate = 1.0; for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); publishObjects(objects); } for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); } } @@ -677,12 +690,14 @@ TEST_F(EvalTest, testYawRate_5) const double yaw_rate = 5.0; for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); publishObjects(objects); } for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); } } @@ -695,12 +710,14 @@ TEST_F(EvalTest, testYawRate_minus_5) const double yaw_rate = 5.0; for (double time = 0; time <= time_delay_; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); publishObjects(objects); } for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { - const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + const auto objects = rotateObjects( + makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); } } diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 7f2f446ae1bee..cfc09459a5cb1 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -2,6 +2,7 @@ + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 3042774d16fd3..c09443be8cab6 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -21,6 +21,11 @@ + + + + + @@ -91,6 +96,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index f502aef017979..a307b192d7caa 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -281,7 +281,7 @@ def create_single_frame_obstacle_segmentation_components(self, input_topic, outp use_additional = bool(additional_lidars) relay_topic = "all_lidars/pointcloud" common_pipeline_output = ( - "single_frame/pointcloud" if use_additional or use_ransac else output_topic + "common/pointcloud" if use_additional or use_ransac else output_topic ) components = self.create_common_pipeline( diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 4af7b3abaa2e6..76fb66e4ebfdf 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -127,6 +127,8 @@ + + diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index f29d6261fc02b..ca56a93cec859 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -48,7 +48,8 @@ MapUpdateModule::MapUpdateModule( bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) { if (last_update_position_ == std::nullopt) { - return false; + need_rebuild_ = true; + return true; } const double dx = position.x - last_update_position_.value().x; diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index 88acbfb52611d..90ec78257e6cb 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -1,5 +1,9 @@ /**: ros__parameters: + user_defined_initial_pose: + enable: $(var user_defined_initial_pose/enable) + pose: $(var user_defined_initial_pose/pose) + gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] ekf_enabled: $(var ekf_enabled) diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json index ced4dc78b93b1..d5b92c45d3038 100644 --- a/localization/pose_initializer/schema/pose_initializer.schema.json +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -6,6 +6,23 @@ "pose_initializer": { "type": "object", "properties": { + "user_defined_initial_pose": { + "type": "object", + "properties": { + "enable": { + "type": "string", + "description": "If true, user_defined_initial_pose.pose is set as the initial position. [boolean]", + "default": "false" + }, + "pose": { + "type": "string", + "description": "initial pose (x, y, z, quat_x, quat_y, quat_z, quat_w). [array]", + "default": "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]" + } + }, + "required": ["enable", "pose"], + "additionalProperties": false + }, "gnss_pose_timeout": { "type": "number", "description": "The duration that the GNSS pose is valid. [sec]", @@ -55,6 +72,7 @@ } }, "required": [ + "user_defined_initial_pose", "gnss_pose_timeout", "stop_check_duration", "ekf_enabled", diff --git a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp index ac9796b687637..9ba424f8e857f 100644 --- a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp @@ -23,13 +23,20 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; -EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) -: logger_(node->get_logger()) +EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) : node_(node) { - client_ekf_trigger_ = node->create_client("ekf_trigger_node"); + client_ekf_trigger_ = node_->create_client("ekf_trigger_node"); } -void EkfLocalizationTriggerModule::send_request(bool flag) const +void EkfLocalizationTriggerModule::wait_for_service() +{ + while (!client_ekf_trigger_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO(node_->get_logger(), "EKF triggering service is not available, waiting..."); + } + RCLCPP_INFO(node_->get_logger(), "EKF triggering service is available!"); +} + +void EkfLocalizationTriggerModule::send_request(bool flag, bool need_spin) const { const auto req = std::make_shared(); std::string command_name; @@ -46,10 +53,14 @@ void EkfLocalizationTriggerModule::send_request(bool flag) const auto future_ekf = client_ekf_trigger_->async_send_request(req); + if (need_spin) { + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_ekf); + } + if (future_ekf.get()->success) { - RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "EKF %s succeeded", command_name.c_str()); } else { - RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "EKF %s failed", command_name.c_str()); throw ServiceException( Initialize::Service::Response::ERROR_ESTIMATION, "EKF " + command_name + " failed"); } diff --git a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp index d1b8eb986105f..901e4ec4414b5 100644 --- a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp @@ -26,10 +26,11 @@ class EkfLocalizationTriggerModule public: explicit EkfLocalizationTriggerModule(rclcpp::Node * node); - void send_request(bool flag) const; + void wait_for_service(); + void send_request(bool flag, bool need_spin = false) const; private: - rclcpp::Logger logger_; + rclcpp::Node * node_; rclcpp::Client::SharedPtr client_ekf_trigger_; }; diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp index e1285f5c31c83..436b1e2df3b21 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp @@ -23,13 +23,20 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; -NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) -: logger_(node->get_logger()) +NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) : node_(node) { - client_ndt_trigger_ = node->create_client("ndt_trigger_node"); + client_ndt_trigger_ = node_->create_client("ndt_trigger_node"); } -void NdtLocalizationTriggerModule::send_request(bool flag) const +void NdtLocalizationTriggerModule::wait_for_service() +{ + while (!client_ndt_trigger_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO(node_->get_logger(), "NDT triggering service is not available, waiting..."); + } + RCLCPP_INFO(node_->get_logger(), "NDT triggering service is available!"); +} + +void NdtLocalizationTriggerModule::send_request(bool flag, bool need_spin) const { const auto req = std::make_shared(); std::string command_name; @@ -46,10 +53,14 @@ void NdtLocalizationTriggerModule::send_request(bool flag) const auto future_ndt = client_ndt_trigger_->async_send_request(req); + if (need_spin) { + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_ndt); + } + if (future_ndt.get()->success) { - RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "NDT %s succeeded", command_name.c_str()); } else { - RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "NDT %s failed", command_name.c_str()); throw ServiceException( Initialize::Service::Response::ERROR_ESTIMATION, "NDT " + command_name + " failed"); } diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp index 91e37c9bb90e9..1c820557fb8ff 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp @@ -26,10 +26,11 @@ class NdtLocalizationTriggerModule public: explicit NdtLocalizationTriggerModule(rclcpp::Node * node); - void send_request(bool flag) const; + void wait_for_service(); + void send_request(bool flag, bool need_spin = false) const; private: - rclcpp::Logger logger_; + rclcpp::Node * node_; rclcpp::Client::SharedPtr client_ndt_trigger_; }; diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 4585414c20b0d..bc86b5476dcca 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -57,6 +57,31 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); + + if (declare_parameter("user_defined_initial_pose.enable")) { + const auto initial_pose_array = + declare_parameter>("user_defined_initial_pose.pose"); + if (initial_pose_array.size() != 7) { + throw std::invalid_argument( + "Could not set user defined initial pose. The size of initial_pose is " + + std::to_string(initial_pose_array.size()) + ". It must be 7."); + } else if ( + std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 && + std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) { + throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero."); + } else { + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = initial_pose_array[0]; + initial_pose.position.y = initial_pose_array[1]; + initial_pose.position.z = initial_pose_array[2]; + initial_pose.orientation.x = initial_pose_array[3]; + initial_pose.orientation.y = initial_pose_array[4]; + initial_pose.orientation.z = initial_pose_array[5]; + initial_pose.orientation.w = initial_pose_array[6]; + + set_user_defined_initial_pose(initial_pose); + } + } } PoseInitializer::~PoseInitializer() @@ -71,6 +96,47 @@ void PoseInitializer::change_state(State::Message::_state_type state) pub_state_->publish(state_); } +// To execute in the constructor, you need to call ros spin. +// Conversely, ros spin should not be called elsewhere +void PoseInitializer::change_node_trigger(bool flag, bool need_spin) +{ + try { + if (ekf_localization_trigger_) { + ekf_localization_trigger_->wait_for_service(); + ekf_localization_trigger_->send_request(flag, need_spin); + } + if (ndt_localization_trigger_) { + ndt_localization_trigger_->wait_for_service(); + ndt_localization_trigger_->send_request(flag, need_spin); + } + } catch (const ServiceException & error) { + throw; + } +} + +void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose) +{ + try { + change_state(State::Message::INITIALIZING); + change_node_trigger(false, true); + + PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = now(); + pose.pose.pose = initial_pose; + pose.pose.covariance = output_pose_covariance_; + pub_reset_->publish(pose); + + change_node_trigger(true, true); + change_state(State::Message::INITIALIZED); + + RCLCPP_INFO(get_logger(), "Set user defined initial pose"); + } catch (const ServiceException & error) { + change_state(State::Message::UNINITIALIZED); + RCLCPP_WARN(get_logger(), "Could not set user defined initial pose"); + } +} + void PoseInitializer::on_initialize( const Initialize::Service::Request::SharedPtr req, const Initialize::Service::Response::SharedPtr res) @@ -82,12 +148,8 @@ void PoseInitializer::on_initialize( } try { change_state(State::Message::INITIALIZING); - if (ekf_localization_trigger_) { - ekf_localization_trigger_->send_request(false); - } - if (ndt_localization_trigger_) { - ndt_localization_trigger_->send_request(false); - } + change_node_trigger(false, false); + auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); if (ndt_) { pose = ndt_->align_pose(pose); @@ -98,12 +160,8 @@ void PoseInitializer::on_initialize( } pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - if (ekf_localization_trigger_) { - ekf_localization_trigger_->send_request(true); - } - if (ndt_localization_trigger_) { - ndt_localization_trigger_->send_request(true); - } + + change_node_trigger(true, false); res->status.success = true; change_state(State::Message::INITIALIZED); } catch (const ServiceException & error) { diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 014c8e9bf6e04..623cfe50307f5 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -58,6 +58,9 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; double stop_check_duration_; + + void change_node_trigger(bool flag, bool need_spin = false); + void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose); void change_state(State::Message::_state_type state); void on_initialize( const Initialize::Service::Request::SharedPtr req, diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index ec13df010c8c3..b8540550ce9da 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index ccec68486b79c..c7697038cc253 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index 200504d0ea18f..f75beddc6827c 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index 7883ae6aa3c99..765f3cde04679 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index e39de639a6121..db2e6ec3a7901 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -327,15 +327,19 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( // objects_pub_->publish(*input_objects); return; } + bool validation_is_ready = true; if (!validator_->setKdtreeInputCloud(input_obstacle_pointcloud)) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); - return; + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5, + "obstacle pointcloud is empty! Can not validate objects."); + validation_is_ready = false; } for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); const auto & object = input_objects->objects.at(i); - const auto validated = validator_->validate_object(transformed_object); + const auto validated = + validation_is_ready ? validator_->validate_object(transformed_object) : false; if (debugger_) { debugger_->addNeighborPointcloud(validator_->getDebugNeighborPointCloud()); debugger_->addPointcloudWithinPolygon(validator_->getDebugPointCloudWithinObject()); diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 756882391183e..594ef1fe974b2 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -33,3 +33,4 @@ center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 use_recheck_ground_cluster: true + use_lowest_point: true diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml index bc02a1ab7e6e7..35d494a620b19 100644 --- a/perception/ground_segmentation/config/scan_ground_filter.param.yaml +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -15,3 +15,4 @@ center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 use_recheck_ground_cluster: true + use_lowest_point: true diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 21df6fa6ea1b9..d4eb9c6f3addf 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | | `elevation_grid_mode` | bool | true | Elevation grid scan mode option | | `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | +| `use_lowest_point` | bool | true | to select lowest point for reference in recheck ground cluster, otherwise select middle point | ## Assumptions / Known limits diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index d017d06929702..d97bbaa2118e5 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -194,6 +194,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter split_height_distance_; // useful for close points bool use_virtual_ground_point_; 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_; @@ -258,7 +260,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * @param non_ground_indices Output non-ground PointCloud indices */ void recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, + PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, pcl::PointIndices & non_ground_indices); /*! * Returns the resulting complementary PointCloud, one with the points kept diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 7a0333d95075b..34573b564fa36 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); 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_ = VehicleInfoUtil(*this).getVehicleInfo(); @@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid( } void ScanGroundFilterComponent::recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, + PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, pcl::PointIndices & non_ground_indices) { - const float min_gnd_height = gnd_cluster.getMinHeight(); + float reference_height = + use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight(); const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { - if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { + if (height_list.at(i) >= reference_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } @@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud if (use_recheck_ground_cluster_) { - recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); + recheckGroundCluster( + ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); } curr_gnd_grid.radius = ground_cluster.getAverageRadius(); curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 444a38ea44754..e48bd36d8c54e 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -83,6 +83,7 @@ class ScanGroundFilterTest : public ::testing::Test rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); parameters.emplace_back( rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_)); options.parameter_overrides(parameters); @@ -157,6 +158,7 @@ class ScanGroundFilterTest : public ::testing::Test center_pcl_shift_ = params["center_pcl_shift"].as(); radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); + use_lowest_point_ = params["use_lowest_point"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -174,6 +176,7 @@ class ScanGroundFilterTest : public ::testing::Test float center_pcl_shift_; float radial_divider_angle_deg_; bool use_recheck_ground_cluster_; + bool use_lowest_point_; }; TEST_F(ScanGroundFilterTest, TestCase1) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index e12b7c54c3a9f..56dc1c2293583 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1360,7 +1360,7 @@ void MapBasedPredictionNode::removeOldObjectsHistory( const double latest_object_time = rclcpp::Time(object_data.back().header.stamp).seconds(); // Delete Old Objects - if (current_time - latest_object_time > 2.0) { + if (current_time - latest_object_time > object_buffer_time_length_) { invalid_object_id.push_back(object_id); continue; } @@ -1368,7 +1368,7 @@ void MapBasedPredictionNode::removeOldObjectsHistory( // Delete old information while (!object_data.empty()) { const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds(); - if (current_time - post_object_time > 2.0) { + if (current_time - post_object_time > object_buffer_time_length_) { // Delete Old Position object_data.pop_front(); } else { diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index fe90be8c583c2..41d150ef0ba1b 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,6 +22,8 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp + src/debugger.cpp + src/processor/processor.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/tracker/motion_model/motion_model_base.cpp @@ -46,12 +48,14 @@ ament_auto_add_library(multi_object_tracker_node SHARED target_link_libraries(multi_object_tracker_node Eigen3::Eigen - glog::glog ) -rclcpp_components_register_node(multi_object_tracker_node - PLUGIN "MultiObjectTracker" - EXECUTABLE multi_object_tracker +ament_auto_add_executable(${PROJECT_NAME} + src/multi_object_tracker_node.cpp +) + +target_link_libraries(${PROJECT_NAME} + multi_object_tracker_node glog ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp new file mode 100644 index 0000000000000..90291ae6fec18 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp @@ -0,0 +1,75 @@ +// 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 MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +/** + * @brief Debugger class for multi object tracker + * @details This class is used to publish debug information of multi object tracker + */ +class TrackerDebugger +{ +public: + explicit TrackerDebugger(rclcpp::Node & node); + void publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void startMeasurementTime( + const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); + void endMeasurementTime(const rclcpp::Time & now); + void startPublishTime(const rclcpp::Time & now); + void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + + void setupDiagnostics(); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + struct DEBUG_SETTINGS + { + bool publish_processing_time; + bool publish_tentative_objects; + double diagnostics_warn_delay; + double diagnostics_error_delay; + } debug_settings_; + double pipeline_latency_ms_ = 0.0; + diagnostic_updater::Updater diagnostic_updater_; + bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } + +private: + void loadParameters(); + bool is_initialized_ = false; + rclcpp::Node & node_; + rclcpp::Publisher::SharedPtr + debug_tentative_objects_pub_; + std::unique_ptr processing_time_publisher_; + rclcpp::Time last_input_stamp_; + rclcpp::Time stamp_process_start_; + rclcpp::Time stamp_process_end_; + rclcpp::Time stamp_publish_start_; + rclcpp::Time stamp_publish_output_; +}; + +#endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index ca59f994a3b0e..f788dd3895216 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -20,12 +20,11 @@ #define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ #include "multi_object_tracker/data_association/data_association.hpp" +#include "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/processor/processor.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include -#include -#include -#include #include #include @@ -41,9 +40,6 @@ #include #endif -#include -#include - #include #include @@ -51,82 +47,44 @@ #include #include #include +#include #include -/** - * @brief Debugger class for multi object tracker - * @details This class is used to publish debug information of multi object tracker - */ -class TrackerDebugger -{ -public: - explicit TrackerDebugger(rclcpp::Node & node); - void publishProcessingTime(); - void publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; - void startStopWatch(); - void startMeasurementTime(const rclcpp::Time & measurement_header_stamp); - void setupDiagnostics(); - void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); - struct DEBUG_SETTINGS - { - bool publish_processing_time; - bool publish_tentative_objects; - double diagnostics_warn_delay; - double diagnostics_error_delay; - } debug_settings_; - double elapsed_time_from_sensor_input_ = 0.0; - diagnostic_updater::Updater diagnostic_updater_; - -private: - void loadParameters(); - rclcpp::Node & node_; - rclcpp::Publisher::SharedPtr - debug_tentative_objects_pub_; - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr processing_time_publisher_; - rclcpp::Time last_input_stamp_; -}; - class MultiObjectTracker : public rclcpp::Node { public: explicit MultiObjectTracker(const rclcpp::NodeOptions & node_options); private: + // ROS interface rclcpp::Publisher::SharedPtr tracked_objects_pub_; rclcpp::Subscription::SharedPtr detected_object_sub_; - rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer - - // debugger class - std::unique_ptr debugger_; - tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::map tracker_map_; - + // debugger + std::unique_ptr debugger_; std::unique_ptr published_time_publisher_; - void onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); - void onTimer(); + // publish timer + rclcpp::TimerBase::SharedPtr publish_timer_; + rclcpp::Time last_published_time_; + double publisher_period_; + // internal states std::string world_frame_id_; // tracking frame - std::list> list_tracker_; std::unique_ptr data_association_; + std::unique_ptr processor_; - void checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform); - void sanitizeTracker( - std::list> & list_tracker, const rclcpp::Time & time); - std::shared_ptr createNewTracker( - const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const; + // callback functions + void onMeasurement( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); + void onTimer(); + // publish processes + void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp new file mode 100644 index 0000000000000..6d6e364d83a41 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -0,0 +1,79 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// + +#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ + +#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +class TrackerProcessor +{ +public: + explicit TrackerProcessor(const std::map & tracker_map); + + const std::list> & getListTracker() const { return list_tracker_; } + // tracker processes + void predict(const rclcpp::Time & time); + void update( + const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects, + const geometry_msgs::msg::Transform & self_transform, + const std::unordered_map & direct_assignment); + void spawn( + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const geometry_msgs::msg::Transform & self_transform, + const std::unordered_map & reverse_assignment); + void prune(const rclcpp::Time & time); + + // output + bool isConfidentTracker(const std::shared_ptr & tracker) const; + void getTrackedObjects( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const; + void getTentativeObjects( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + +private: + std::map tracker_map_; + std::list> list_tracker_; + + // parameters + float max_elapsed_time_; // [s] + float min_iou_; // [ratio] + float min_iou_for_unknown_object_; // [ratio] + double distance_threshold_; // [m] + int confident_count_threshold_; // [count] + + void removeOldTracker(const rclcpp::Time & time); + void removeOverlappedTracker(const rclcpp::Time & time); + std::shared_ptr createNewTracker( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) const; +}; + +#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 699b28222f63f..4033f85eafb8a 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -25,7 +25,6 @@ tf2 tf2_ros tier4_autoware_utils - tier4_perception_msgs unique_identifier_msgs ament_lint_auto diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger.cpp new file mode 100644 index 0000000000000..b5632a13e78fc --- /dev/null +++ b/perception/multi_object_tracker/src/debugger.cpp @@ -0,0 +1,169 @@ +// 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 "multi_object_tracker/debugger.hpp" + +#include + +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) +{ + // declare debug parameters to decide whether to publish debug topics + loadParameters(); + // initialize debug publishers + if (debug_settings_.publish_processing_time) { + processing_time_publisher_ = + std::make_unique(&node_, "multi_object_tracker"); + } + + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_ = + node_.create_publisher( + "debug/tentative_objects", rclcpp::QoS{1}); + } + + // initialize timestamps + const rclcpp::Time now = node_.now(); + last_input_stamp_ = now; + stamp_process_start_ = now; + stamp_process_end_ = now; + stamp_publish_start_ = now; + stamp_publish_output_ = now; + + // setup diagnostics + setupDiagnostics(); +} + +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); +} + +void TrackerDebugger::loadParameters() +{ + try { + debug_settings_.publish_processing_time = + node_.declare_parameter("publish_processing_time"); + debug_settings_.publish_tentative_objects = + node_.declare_parameter("publish_tentative_objects"); + debug_settings_.diagnostics_warn_delay = + node_.declare_parameter("diagnostics_warn_delay"); + debug_settings_.diagnostics_error_delay = + node_.declare_parameter("diagnostics_error_delay"); + } catch (const std::exception & e) { + RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); + debug_settings_.publish_processing_time = false; + debug_settings_.publish_tentative_objects = false; + debug_settings_.diagnostics_warn_delay = 0.5; + debug_settings_.diagnostics_error_delay = 1.0; + } +} + +void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (!is_initialized_) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Measurement time is not set."); + return; + } + const double & delay = pipeline_latency_ms_; // [s] + + if (delay == 0.0) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); + } else if (delay < debug_settings_.diagnostics_warn_delay) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); + } else if (delay < debug_settings_.diagnostics_error_delay) { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); + } else { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); + } + + stat.add("Detection delay", delay); +} + +void TrackerDebugger::publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +void TrackerDebugger::startMeasurementTime( + const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp) +{ + last_input_stamp_ = measurement_header_stamp; + stamp_process_start_ = now; + if (debug_settings_.publish_processing_time) { + double input_latency_ms = (now - last_input_stamp_).seconds() * 1e3; + processing_time_publisher_->publish( + "debug/input_latency_ms", input_latency_ms); + } + // initialize debug time stamps + if (!is_initialized_) { + stamp_publish_output_ = now; + is_initialized_ = true; + } +} + +void TrackerDebugger::endMeasurementTime(const rclcpp::Time & now) +{ + stamp_process_end_ = now; +} + +void TrackerDebugger::startPublishTime(const rclcpp::Time & now) +{ + stamp_publish_start_ = now; +} + +void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time) +{ + // if the measurement time is not set, do not publish debug information + if (!is_initialized_) return; + + // pipeline latency: time from sensor measurement to publish, used for 'checkDelay' + pipeline_latency_ms_ = (now - last_input_stamp_).seconds() * 1e3; + + if (debug_settings_.publish_processing_time) { + // processing latency: time between input and publish + double processing_latency_ms = ((now - stamp_process_start_).seconds()) * 1e3; + // processing time: only the time spent in the tracking processes + double processing_time_ms = ((now - stamp_publish_start_).seconds() + + (stamp_process_end_ - stamp_process_start_).seconds()) * + 1e3; + // cycle time: time between two consecutive publish + double cyclic_time_ms = (now - stamp_publish_output_).seconds() * 1e3; + // measurement to tracked-object time: time from the sensor measurement to the publishing object + // time If there is not latency compensation, this value is zero + double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3; + + // starting from the measurement time + processing_time_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms_); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + processing_time_publisher_->publish( + "debug/processing_latency_ms", processing_latency_ms); + processing_time_publisher_->publish( + "debug/meas_to_tracked_object_ms", measurement_to_object_ms); + } + stamp_publish_output_ = now; +} diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index fa609ed9ff9c7..d80a21813b28b 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -31,7 +31,6 @@ #define EIGEN_MPL2_ONLY #include "multi_object_tracker/multi_object_tracker_core.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -41,18 +40,20 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; namespace { +// Function to get the transform between two frames boost::optional getTransformAnonymous( const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, const std::string & target_frame_id, const rclcpp::Time & time) { try { - // check if the frames are ready + // Check if the frames are ready std::string errstr; // This argument prevents error msg from being displayed in the terminal. if (!tf_buffer.canTransform( target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { return boost::none; } + // Lookup the transform geometry_msgs::msg::TransformStamped self_transform_stamped; self_transform_stamped = tf_buffer.lookupTransform( /*target*/ target_frame_id, /*src*/ source_frame_id, time, @@ -66,122 +67,12 @@ boost::optional getTransformAnonymous( } // namespace -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) -: diagnostic_updater_(&node), node_(node), last_input_stamp_(node.now()) -{ - // declare debug parameters to decide whether to publish debug topics - loadParameters(); - // initialize debug publishers - stop_watch_ptr_ = std::make_unique>(); - if (debug_settings_.publish_processing_time) { - processing_time_publisher_ = - std::make_unique(&node_, "multi_object_tracker"); - } - - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_ = - node_.create_publisher( - "debug/tentative_objects", rclcpp::QoS{1}); - } - - // initialize stop watch and diagnostics - startStopWatch(); - setupDiagnostics(); -} - -void TrackerDebugger::setupDiagnostics() -{ - diagnostic_updater_.setHardwareID(node_.get_name()); - diagnostic_updater_.add( - "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); - diagnostic_updater_.setPeriod(0.1); -} - -void TrackerDebugger::loadParameters() -{ - try { - debug_settings_.publish_processing_time = - node_.declare_parameter("publish_processing_time"); - debug_settings_.publish_tentative_objects = - node_.declare_parameter("publish_tentative_objects"); - debug_settings_.diagnostics_warn_delay = - node_.declare_parameter("diagnostics_warn_delay"); - debug_settings_.diagnostics_error_delay = - node_.declare_parameter("diagnostics_error_delay"); - } catch (const std::exception & e) { - RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); - debug_settings_.publish_processing_time = false; - debug_settings_.publish_tentative_objects = false; - debug_settings_.diagnostics_warn_delay = 0.5; - debug_settings_.diagnostics_error_delay = 1.0; - } -} - -void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - const double delay = elapsed_time_from_sensor_input_; // [s] - - if (delay == 0.0) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); - } else if (delay < debug_settings_.diagnostics_warn_delay) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); - } else if (delay < debug_settings_.diagnostics_error_delay) { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); - } else { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); - } - - stat.add("Detection delay", delay); -} - -void TrackerDebugger::publishProcessingTime() -{ - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const auto current_time = node_.now(); - elapsed_time_from_sensor_input_ = (current_time - last_input_stamp_).seconds(); - if (debug_settings_.publish_processing_time) { - processing_time_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_publisher_->publish( - "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3); - } -} - -void TrackerDebugger::publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const -{ - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_->publish(tentative_objects); - } -} - -void TrackerDebugger::startStopWatch() -{ - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); -} - -void TrackerDebugger::startMeasurementTime(const rclcpp::Time & measurement_header_stamp) -{ - last_input_stamp_ = measurement_header_stamp; - // start measuring processing time - stop_watch_ptr_->toc("processing_time", true); -} - MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_) + tf_listener_(tf_buffer_), + last_published_time_(this->now()) { - // glog for debug - google::InitGoogleLogging("multi_object_tracker"); - google::InstallFailureSignalHandler(); - // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, @@ -189,261 +80,154 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); - // Parameters - double publish_rate = declare_parameter("publish_rate"); + // Get parameters + double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; - // Debug publishers - debugger_ = std::make_unique(*this); - + // Create tf timer auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_.setCreateTimerInterface(cti); - // Create ROS time based timer + // Create ROS time based timer. + // If the delay compensation is enabled, the timer is used to publish the output at the correct + // time. if (enable_delay_compensation) { - const auto period_ns = rclcpp::Rate(publish_rate).period(); + publisher_period_ = 1.0 / publish_rate; // [s] + constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check + const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); publish_timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this)); + this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); } - const auto tmp = this->declare_parameter>("can_assign_matrix"); - const std::vector can_assign_matrix(tmp.begin(), tmp.end()); - - const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); - const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); - const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); - const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - - // tracker map - tracker_map_.insert( - std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); - tracker_map_.insert( - std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); - tracker_map_.insert( - std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); - tracker_map_.insert( - std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); - tracker_map_.insert( - std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); - tracker_map_.insert( - std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); - tracker_map_.insert( - std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); - - data_association_ = std::make_unique( - can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, - min_iou_matrix); + // Initialize processor + { + std::map tracker_map; + tracker_map.insert( + std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); + tracker_map.insert( + std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); + tracker_map.insert( + std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map.insert( + std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); + tracker_map.insert(std::make_pair( + Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); + tracker_map.insert( + std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); + tracker_map.insert(std::make_pair( + Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + + processor_ = std::make_unique(tracker_map); + } + + // Data association initialization + { + const auto tmp = this->declare_parameter>("can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); + const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); + + data_association_ = std::make_unique( + can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, + min_iou_matrix); + } + + // Debugger + debugger_ = std::make_unique(*this); published_time_publisher_ = std::make_unique(this); } void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { + // Get the time of the measurement + const rclcpp::Time measurement_time = + rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type()); + /* keep the latest input stamp and check transform*/ - debugger_->startMeasurementTime(rclcpp::Time(input_objects_msg->header.stamp)); - const auto self_transform = getTransformAnonymous( - tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); + debugger_->startMeasurementTime(this->now(), measurement_time); + const auto self_transform = + getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); if (!self_transform) { return; } - /* transform to world coordinate */ autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } - /* tracker prediction */ - const rclcpp::Time measurement_time = input_objects_msg->header.stamp; - for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - (*itr)->predict(measurement_time); - } - /* global nearest neighbor */ + ////// Tracker Process + //// Associate and update + /* prediction */ + processor_->predict(measurement_time); + /* object association */ std::unordered_map direct_assignment, reverse_assignment; - Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( - transformed_objects, list_tracker_); // row : tracker, col : measurement - data_association_->assign(score_matrix, direct_assignment, reverse_assignment); - - /* tracker measurement update */ - int tracker_idx = 0; - for (auto tracker_itr = list_tracker_.begin(); tracker_itr != list_tracker_.end(); - ++tracker_itr, ++tracker_idx) { - if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found - (*(tracker_itr)) - ->updateWithMeasurement( - transformed_objects.objects.at(direct_assignment.find(tracker_idx)->second), - measurement_time, *self_transform); - } else { // not found - (*(tracker_itr))->updateWithoutMeasurement(); - } + { + const auto & list_tracker = processor_->getListTracker(); + const auto & detected_objects = transformed_objects; + // global nearest neighbor + Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + detected_objects, list_tracker); // row : tracker, col : measurement + data_association_->assign(score_matrix, direct_assignment, reverse_assignment); } + /* tracker update */ + processor_->update(transformed_objects, *self_transform, direct_assignment); + /* tracker pruning */ + processor_->prune(measurement_time); + /* spawn new tracker */ + processor_->spawn(transformed_objects, *self_transform, reverse_assignment); - /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); - /* sanitize trackers */ - sanitizeTracker(list_tracker_, measurement_time); - - /* new tracker */ - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { - if (reverse_assignment.find(i) != reverse_assignment.end()) { // found - continue; - } - std::shared_ptr tracker = - createNewTracker(transformed_objects.objects.at(i), measurement_time, *self_transform); - if (tracker) list_tracker_.push_back(tracker); - } + // debugger time + debugger_->endMeasurementTime(this->now()); + // Publish objects if the timer is not enabled if (publish_timer_ == nullptr) { + // Publish if the delay compensation is disabled publish(measurement_time); - } -} - -std::shared_ptr MultiObjectTracker::createNewTracker( - const autoware_auto_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); - if (tracker_map_.count(label) != 0) { - const auto tracker = tracker_map_.at(label); - - if (tracker == "bicycle_tracker") { - return std::make_shared(time, object, self_transform); - } else if (tracker == "big_vehicle_tracker") { - return std::make_shared(time, object, self_transform); - } else if (tracker == "multi_vehicle_tracker") { - return std::make_shared(time, object, self_transform); - } else if (tracker == "normal_vehicle_tracker") { - return std::make_shared(time, object, self_transform); - } else if (tracker == "pass_through_tracker") { - return std::make_shared(time, object, self_transform); - } else if (tracker == "pedestrian_and_bicycle_tracker") { - return std::make_shared(time, object, self_transform); - } else if (tracker == "pedestrian_tracker") { - return std::make_shared(time, object, self_transform); + } else { + // Publish if the next publish time is close + const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period + if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(this->now()); } } - return std::make_shared(time, object, self_transform); } void MultiObjectTracker::onTimer() { const rclcpp::Time current_time = this->now(); - const auto self_transform = - getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); - if (!self_transform) { - return; + // Check the publish period + const auto elapsed_time = (current_time - last_published_time_).seconds(); + // If the elapsed time is over the period, publish objects with prediction + constexpr double maximum_latency_ratio = 1.11; // 11% margin + const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; + if (elapsed_time > maximum_publish_latency) { + checkAndPublish(current_time); } - - /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); - /* sanitize trackers */ - sanitizeTracker(list_tracker_, current_time); - - // Publish - publish(current_time); } -void MultiObjectTracker::checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) +void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) { - /* params */ - constexpr float max_elapsed_time = 1.0; - - /* delete tracker */ - for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { - const bool is_old = max_elapsed_time < (*itr)->getElapsedTimeFromLastUpdate(time); - if (is_old) { - auto erase_itr = itr; - --itr; - list_tracker.erase(erase_itr); - } - } -} + /* tracker pruning*/ + processor_->prune(time); -void MultiObjectTracker::sanitizeTracker( - std::list> & list_tracker, const rclcpp::Time & time) -{ - constexpr float min_iou = 0.1; - constexpr float min_iou_for_unknown_object = 0.001; - constexpr double distance_threshold = 5.0; - /* delete collision tracker */ - for (auto itr1 = list_tracker.begin(); itr1 != list_tracker.end(); ++itr1) { - autoware_auto_perception_msgs::msg::TrackedObject object1; - if (!(*itr1)->getTrackedObject(time, object1)) continue; - for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { - autoware_auto_perception_msgs::msg::TrackedObject object2; - if (!(*itr2)->getTrackedObject(time, object2)) continue; - const double distance = std::hypot( - object1.kinematics.pose_with_covariance.pose.position.x - - object2.kinematics.pose_with_covariance.pose.position.x, - object1.kinematics.pose_with_covariance.pose.position.y - - object2.kinematics.pose_with_covariance.pose.position.y); - if (distance_threshold < distance) { - continue; - } - - const double min_union_iou_area = 1e-2; - const auto iou = 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; - bool should_delete_tracker2 = false; - - // If at least one of them is UNKNOWN, delete the one with lower IOU. Because the UNKNOWN - // objects are not reliable. - if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { - if (min_iou_for_unknown_object < iou) { - if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { - if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { - should_delete_tracker1 = true; - } else { - should_delete_tracker2 = true; - } - } else if (label1 == Label::UNKNOWN) { - should_delete_tracker1 = true; - } else if (label2 == Label::UNKNOWN) { - should_delete_tracker2 = true; - } - } - } else { // If neither is UNKNOWN, delete the one with lower IOU. - if (min_iou < iou) { - if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { - should_delete_tracker1 = true; - } else { - should_delete_tracker2 = true; - } - } - } - - if (should_delete_tracker1) { - itr1 = list_tracker.erase(itr1); - --itr1; - break; - } else if (should_delete_tracker2) { - itr2 = list_tracker.erase(itr2); - --itr2; - } - } - } -} + // Publish + publish(time); -inline bool MultiObjectTracker::shouldTrackerPublish( - const std::shared_ptr tracker) const -{ - constexpr int measurement_count_threshold = 3; - if (tracker->getTotalMeasurementCount() < measurement_count_threshold) { - return false; - } - return true; + // Update last published time + last_published_time_ = this->now(); } void MultiObjectTracker::publish(const rclcpp::Time & time) const { + debugger_->startPublishTime(this->now()); const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); if (subscriber_count < 1) { @@ -452,28 +236,21 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const // Create output msg autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; - output_msg.header.stamp = time; - tentative_objects_msg.header = output_msg.header; - - for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - if (!shouldTrackerPublish(*itr)) { // for debug purpose - autoware_auto_perception_msgs::msg::TrackedObject object; - if (!(*itr)->getTrackedObject(time, object)) continue; - tentative_objects_msg.objects.push_back(object); - continue; - } - autoware_auto_perception_msgs::msg::TrackedObject object; - if (!(*itr)->getTrackedObject(time, object)) continue; - output_msg.objects.push_back(object); - } + processor_->getTrackedObjects(time, output_msg); // Publish tracked_objects_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(tracked_objects_pub_, output_msg.header.stamp); - // Debugger Publish if enabled - debugger_->publishProcessingTime(); - debugger_->publishTentativeObjects(tentative_objects_msg); + // Publish debugger information if enabled + debugger_->endPublishTime(this->now(), time); + + if (debugger_->shouldPublishTentativeObjects()) { + autoware_auto_perception_msgs::msg::TrackedObjects tentative_objects_msg; + tentative_objects_msg.header.frame_id = world_frame_id_; + processor_->getTentativeObjects(time, tentative_objects_msg); + debugger_->publishTentativeObjects(tentative_objects_msg); + } } RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp new file mode 100644 index 0000000000000..f20aedf16efef --- /dev/null +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -0,0 +1,34 @@ +// 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 "multi_object_tracker/multi_object_tracker_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + google::InitGoogleLogging(argv[0]); // NOLINT + google::InstallFailureSignalHandler(); + + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto multi_object_tracker = std::make_shared(options); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(multi_object_tracker); + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp new file mode 100644 index 0000000000000..0d56e16b431e9 --- /dev/null +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -0,0 +1,242 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// + +#include "multi_object_tracker/processor/processor.hpp" + +#include "multi_object_tracker/tracker/tracker.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +TrackerProcessor::TrackerProcessor(const std::map & tracker_map) +: tracker_map_(tracker_map) +{ + // Set tracker lifetime parameters + max_elapsed_time_ = 1.0; // [s] + + // Set tracker overlap remover parameters + min_iou_ = 0.1; // [ratio] + min_iou_for_unknown_object_ = 0.001; // [ratio] + distance_threshold_ = 5.0; // [m] + + // Set tracker confidence threshold + confident_count_threshold_ = 3; // [count] +} + +void TrackerProcessor::predict(const rclcpp::Time & time) +{ + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + (*itr)->predict(time); + } +} + +void TrackerProcessor::update( + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const geometry_msgs::msg::Transform & self_transform, + const std::unordered_map & direct_assignment) +{ + int tracker_idx = 0; + const auto & time = detected_objects.header.stamp; + for (auto tracker_itr = list_tracker_.begin(); tracker_itr != list_tracker_.end(); + ++tracker_itr, ++tracker_idx) { + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found + const auto & associated_object = + detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); + } else { // not found + (*(tracker_itr))->updateWithoutMeasurement(); + } + } +} + +void TrackerProcessor::spawn( + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const geometry_msgs::msg::Transform & self_transform, + const std::unordered_map & reverse_assignment) +{ + const auto & time = detected_objects.header.stamp; + for (size_t i = 0; i < detected_objects.objects.size(); ++i) { + if (reverse_assignment.find(i) != reverse_assignment.end()) { // found + continue; + } + const auto & new_object = detected_objects.objects.at(i); + std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); + if (tracker) list_tracker_.push_back(tracker); + } +} + +std::shared_ptr TrackerProcessor::createNewTracker( + const autoware_auto_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); + if (tracker_map_.count(label) != 0) { + const auto tracker = tracker_map_.at(label); + if (tracker == "bicycle_tracker") + return std::make_shared(time, object, self_transform); + if (tracker == "big_vehicle_tracker") + return std::make_shared(time, object, self_transform); + if (tracker == "multi_vehicle_tracker") + return std::make_shared(time, object, self_transform); + if (tracker == "normal_vehicle_tracker") + return std::make_shared(time, object, self_transform); + if (tracker == "pass_through_tracker") + return std::make_shared(time, object, self_transform); + if (tracker == "pedestrian_and_bicycle_tracker") + return std::make_shared(time, object, self_transform); + if (tracker == "pedestrian_tracker") + return std::make_shared(time, object, self_transform); + } + return std::make_shared(time, object, self_transform); +} + +void TrackerProcessor::prune(const rclcpp::Time & time) +{ + // Check tracker lifetime: if the tracker is old, delete it + removeOldTracker(time); + // Check tracker overlap: if the tracker is overlapped, delete the one with lower IOU + removeOverlappedTracker(time); +} + +void TrackerProcessor::removeOldTracker(const rclcpp::Time & time) +{ + // Check elapsed time from last update + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { + const bool is_old = max_elapsed_time_ < (*itr)->getElapsedTimeFromLastUpdate(time); + // If the tracker is old, delete it + if (is_old) { + auto erase_itr = itr; + --itr; + list_tracker_.erase(erase_itr); + } + } +} + +// This function removes overlapped trackers based on distance and IoU criteria +void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) +{ + // Iterate through the list of trackers + for (auto itr1 = list_tracker_.begin(); itr1 != list_tracker_.end(); ++itr1) { + autoware_auto_perception_msgs::msg::TrackedObject object1; + if (!(*itr1)->getTrackedObject(time, object1)) continue; + + // Compare the current tracker with the remaining trackers + for (auto itr2 = std::next(itr1); itr2 != list_tracker_.end(); ++itr2) { + autoware_auto_perception_msgs::msg::TrackedObject object2; + if (!(*itr2)->getTrackedObject(time, object2)) continue; + + // Calculate the distance between the two objects + const double distance = std::hypot( + object1.kinematics.pose_with_covariance.pose.position.x - + object2.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y - + object2.kinematics.pose_with_covariance.pose.position.y); + + // If the distance is too large, skip + if (distance > distance_threshold_) { + continue; + } + + // 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 & label1 = (*itr1)->getHighestProbLabel(); + const auto & label2 = (*itr2)->getHighestProbLabel(); + bool should_delete_tracker1 = false; + bool should_delete_tracker2 = false; + + // If both trackers are UNKNOWN, delete the younger tracker + // If one side of the tracker is UNKNOWN, delete UNKNOWN objects + if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { + if (iou > min_iou_for_unknown_object_) { + if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { + if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { + should_delete_tracker1 = true; + } else { + should_delete_tracker2 = true; + } + } else if (label1 == Label::UNKNOWN) { + should_delete_tracker1 = true; + } else if (label2 == Label::UNKNOWN) { + should_delete_tracker2 = true; + } + } + } else { // If neither object is UNKNOWN, delete the younger tracker + if (iou > min_iou_) { + if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { + should_delete_tracker1 = true; + } else { + should_delete_tracker2 = true; + } + } + } + + // Delete the tracker + if (should_delete_tracker1) { + itr1 = list_tracker_.erase(itr1); + --itr1; + break; + } + if (should_delete_tracker2) { + itr2 = list_tracker_.erase(itr2); + --itr2; + } + } + } +} + +bool TrackerProcessor::isConfidentTracker(const std::shared_ptr & tracker) const +{ + // Confidence is determined by counting the number of measurements. + // If the number of measurements is equal to or greater than the threshold, the tracker is + // considered confident. + return tracker->getTotalMeasurementCount() >= confident_count_threshold_; +} + +void TrackerProcessor::getTrackedObjects( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const +{ + tracked_objects.header.stamp = time; + for (const auto & tracker : list_tracker_) { + // Skip if the tracker is not confident + if (!isConfidentTracker(tracker)) continue; + // Get the tracked object, extrapolated to the given time + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + if (tracker->getTrackedObject(time, tracked_object)) { + tracked_objects.objects.push_back(tracked_object); + } + } +} + +void TrackerProcessor::getTentativeObjects( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + tentative_objects.header.stamp = time; + for (const auto & tracker : list_tracker_) { + if (!isConfidentTracker(tracker)) { + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + if (tracker->getTrackedObject(time, tracked_object)) { + tentative_objects.objects.push_back(tracked_object); + } + } + } +} diff --git a/perception/object_range_splitter/README.md b/perception/object_range_splitter/README.md index 89d266a666219..19c571962c0dc 100644 --- a/perception/object_range_splitter/README.md +++ b/perception/object_range_splitter/README.md @@ -43,9 +43,7 @@ Example: ## Parameters -| Name | Type | Description | -| ------------- | ----- | ---------------------------------------------------- | -| `split_range` | float | the distance boundary to divide detected objects [m] | +{{ json_to_markdown("perception/object_range_splitter/schema/object_range_splitter.schema.json") }} ## Assumptions / Known limits diff --git a/perception/object_range_splitter/schema/object_range_splitter.schema.json b/perception/object_range_splitter/schema/object_range_splitter.schema.json new file mode 100644 index 0000000000000..ed5ba3a96c18b --- /dev/null +++ b/perception/object_range_splitter/schema/object_range_splitter.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for object_range_splitter", + "type": "object", + "definitions": { + "object_range_splitter": { + "type": "object", + "properties": { + "split_range": { + "type": "number", + "description": "object_range_splitter is a package to divide detected objects into two messages by the distance from the origin", + "default": "30.0" + } + }, + "required": ["split_range"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/object_range_splitter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/.pages b/planning/.pages index 1b5608f7ded57..942a5a0b32158 100644 --- a/planning/.pages +++ b/planning/.pages @@ -67,7 +67,7 @@ nav: - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja - 'Scenario Selector': planning/scenario_selector - - 'Static Centerline Optimizer': planning/static_centerline_optimizer + - 'Static Centerline Generator': planning/static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - 'External Velocity Limit Selector': planning/external_velocity_limit_selector diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index ad5fe0b123f1d..384ef7bd285e9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -8,8 +8,8 @@ target_object: car: execute_num: 2 # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] + th_moving_speed: 1.0 # [m/s] + th_moving_time: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] lateral_margin: @@ -18,8 +18,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -28,8 +28,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -38,8 +38,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -48,8 +48,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] unknown: execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 # 1.0km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -58,34 +58,34 @@ hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 # 1.0km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 lateral_margin: soft_margin: 0.0 # [m] - hard_margin: 0.0 # [m] - hard_margin_for_parked_vehicle: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] motorcycle: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 lateral_margin: soft_margin: 0.0 # [m] - hard_margin: 0.0 # [m] - hard_margin_for_parked_vehicle: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] pedestrian: execute_num: 2 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 # 1.0km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 lateral_margin: soft_margin: 0.0 # [m] - hard_margin: 0.0 # [m] - hard_margin_for_parked_vehicle: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] @@ -101,11 +101,3 @@ bicycle: false # [-] motorcycle: false # [-] pedestrian: false # [-] - - constraints: - # lateral constraints - lateral: - velocity: [1.0, 1.38, 11.1] # [m/s] - max_accel_values: [0.5, 0.5, 0.5] # [m/ss] - min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] - max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 799364437955c..a3b28b4d63d06 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -65,10 +65,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); @@ -121,14 +119,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "max_compensation_time"); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); } { @@ -137,9 +139,9 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); p.time_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.time_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.distance_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/behavior_path_avoidance_module/README.md index dc6ab0ec4b6e1..aa7a3a42efdb4 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -760,176 +760,7 @@ WIP The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml`. -### General parameters - -namespace: `avoidance.` - -| Name | Unit | Type | Description | Default value | -| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | - -| Name | Unit | Type | Description | Default value | -| :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | -| enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | -| use_adjacent_lane | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | -| use_opposite_lane | [-] | bool | Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects | true | -| use_intersection_areas | [-] | bool | Extend drivable to intersection area. | false | -| use_hatched_road_markings | [-] | bool | Extend drivable to hatched road marking area. | false | - -| Name | Unit | Type | Description | Default value | -| :------------------ | ---- | ---- | --------------------------------------------------------------------------------------- | ------------- | -| output_debug_marker | [-] | bool | Flag to publish debug marker (set `false` as default since it takes considerable cost). | false | -| output_debug_info | [-] | bool | Flag to print debug info (set `false` as default since it takes considerable cost). | false | - -### Avoidance target filtering parameters - -namespace: `avoidance.target_object.` - -This module supports all object classes, and it can set following parameters independently. - -```yaml -car: - is_target: true # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] - max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] - safety_buffer_longitudinal: 0.0 # [m] -``` - -| Name | Unit | Type | Description | Default value | -| :------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------- | ------------- | -| is_target | [-] | bool | By setting this flag `true`, this module avoid those class objects. | false | -| moving_speed_threshold | [m/s] | double | Objects with speed greater than this will be judged as moving ones. | 1.0 | -| moving_time_threshold | [s] | double | Objects keep moving longer duration than this will be excluded from avoidance target. | 1.0 | -| envelope_buffer_margin | [m] | double | The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation. | 0.3 | -| avoid_margin_lateral | [m] | double | The lateral distance between ego and avoidance targets. | 1.0 | -| safety_buffer_lateral | [m] | double | Creates an additional lateral gap that will prevent the vehicle from getting to near to the obstacle. | 0.5 | -| safety_buffer_longitudinal | [m] | double | Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle. | 0.0 | - -Parameters for the logic to compensate perception noise of the far objects. - -| Name | Unit | Type | Description | Default value | -| :----------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------------------ | ------------- | -| max_expand_ratio | [-] | double | This value will be applied `envelope_buffer_margin` according to the distance between the ego and object. | 0.0 | -| lower_distance_for_polygon_expansion | [-] | double | If the distance between the ego and object is less than this, the expand ratio will be zero. | 30.0 | -| upper_distance_for_polygon_expansion | [-] | double | If the distance between the ego and object is larger than this, the expand ratio will be `max_expand_ratio`. | 100.0 | - -namespace: `avoidance.target_filtering.` - -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| threshold_distance_object_is_on_center | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 1.0 | -| object_ignore_section_traffic_light_in_front_distance | [m] | double | If the distance between traffic light and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_ignore_section_crosswalk_in_front_distance | [m] | double | If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | -| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 | -| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 | -| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | -| object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | -| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | - -### Safety check parameters - -namespace: `avoidance.safety_check.` - -| Name | Unit | Type | Description | Default value | -| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| enable | [-] | bool | Enable to use safety check feature. | true | -| check_current_lane | [-] | bool | Check objects on current driving lane. | false | -| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true | -| check_other_side_lane | [-] | bool | Check objects on other side lane. | false | -| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false | -| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false | -| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false | -| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | -| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 | -| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | -| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | -| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | - -### Avoidance maneuver parameters - -namespace: `avoidance.avoidance.lateral.` - -| Name | Unit | Type | Description | Default value | -| :---------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------------------- | :------------ | -| road_shoulder_safety_margin | [m] | double | Prevents the generated path to come too close to the road shoulders. | 0.0 | -| lateral_execution_threshold | [m] | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2) | 0.499 | -| lateral_small_shift_threshold | [m] | double | The shift lines whose lateral offset is less than this will be applied with other ones. | 0.501 | -| max_right_shift_length | [m] | double | Maximum shift length for right direction | 5.0 | -| max_left_shift_length | [m] | double | Maximum shift length for left direction | 5.0 | - -namespace: `avoidance.avoidance.longitudinal.` - -| Name | Unit | Type | Description | Default value | -| :----------------------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | -| longitudinal_collision_safety_buffer | [s] | double | Longitudinal collision buffer between target object and shift line. | 0.0 | -| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | -| min_avoidance_distance | [m] | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low) | 10.0 | -| min_nominal_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1). | 7.0 | -| min_sharp_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1). | 1.0 | -| min_slow_down_speed | [m/s] | double | Minimum slow speed for avoidance prepare section. | 1.38 (5km/h) | -| buf_slow_down_speed | [m/s] | double | Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed. | 0.57 (2.0km/h) | - -### Yield maneuver parameters - -namespace: `avoidance.yield.` - -| Name | Unit | Type | Description | Default value | -| :------------- | :---- | :----- | :------------------------------------------------------------ | :------------ | -| yield_velocity | [m/s] | double | The ego will decelerate yield velocity in the yield maneuver. | 2.78 | - -### Stop maneuver parameters - -namespace: `avoidance.stop.` - -| Name | Unit | Type | Description | Default value | -| :----------- | :--- | :----- | :---------------------------------------------------------------------------------------------------- | :------------ | -| min_distance | [m] | double | Minimum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 10.0 | -| max_distance | [m] | double | Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 20.0 | -| stop_buffer | [m] | double | Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 1.0 | - -### Constraints parameters - -namespace: `avoidance.constraints.` - -| Name | Unit | Type | Description | Default value | -| :------------------------ | :--- | :--- | :--------------------------------------------------------------------------------------- | :------------ | -| use_constraints_for_decel | [-] | bool | Flag to decel under longitudinal constraints. `TRUE: allow to control breaking mildness` | false | - -namespace: `avoidance.constraints.lateral.` - -| a Name | Unit | Type | Description | Default value | -| :----------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------- | :------------ | -| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | -| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | -| nominal_lateral_jerk | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego. | 0.2 | -| max_lateral_jerk | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego. | 1.0 | -| max_lateral_acceleration | [m/s3] | double | Avoidance path gets sharp up to this accel limit when there is not enough distance from ego. | 0.5 | - -namespace: `avoidance.constraints.longitudinal.` - -| Name | Unit | Type | Description | Default value | -| :------------------- | :------ | :----- | :------------------------------------- | :------------ | -| nominal_deceleration | [m/ss] | double | Nominal deceleration limit. | -1.0 | -| nominal_jerk | [m/sss] | double | Nominal jerk limit. | 0.5 | -| max_deceleration | [m/ss] | double | Max decelerate limit. | -2.0 | -| max_jerk | [m/sss] | double | Max jerk limit. | 1.0 | -| max_acceleration | [m/ss] | double | Maximum acceleration during avoidance. | 1.0 | - -(\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value. +{{ json_to_markdown("planning/behavior_path_avoidance_module/schema/avoidance.schema.json") }} ## Future extensions / Unimplemented parts diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 3c3a086a3514d..5d6044df64b2f 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -2,11 +2,11 @@ /**: ros__parameters: avoidance: - resample_interval_for_planning: 0.3 # [m] - resample_interval_for_output: 4.0 # [m] + resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER + resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + # avoidance module common setting enable_bound_clipping: false - enable_cancel_maneuver: true disable_path_update: false # drivable area setting @@ -16,215 +16,195 @@ use_hatched_road_markings: true use_freespace_areas: true - # for debug - publish_debug_marker: false - print_debug_info: false - # avoidance is performed for the object type with true target_object: car: - execute_num: 1 # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 2.0 # [s] + th_moving_speed: 1.0 # [m/s] + th_moving_time: 2.0 # [s] + longitudinal_margin: 0.0 # [m] lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.3 # [m] - hard_margin_for_parked_vehicle: 0.3 # [m] - max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.5 # [m] - safety_buffer_longitudinal: 0.0 # [m] - use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] + max_expand_ratio: 0.0 # [-] FOR DEVELOPER + envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER truck: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 2.0 + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true bus: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 2.0 + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true trailer: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 2.0 + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true unknown: - execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: -0.2 # [m] - hard_margin_for_parked_vehicle: -0.2 # [m] + soft_margin: 0.7 + hard_margin: -0.2 + hard_margin_for_parked_vehicle: -0.2 max_expand_ratio: 0.0 envelope_buffer_margin: 0.1 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true bicycle: - execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.5 # [m] - hard_margin_for_parked_vehicle: 0.5 # [m] + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: true motorcycle: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 + th_moving_time: 1.0 + longitudinal_margin: 1.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.3 # [m] - hard_margin_for_parked_vehicle: 0.3 # [m] + soft_margin: 0.7 + hard_margin: 0.3 + hard_margin_for_parked_vehicle: 0.3 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: true pedestrian: - execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.5 # [m] - hard_margin_for_parked_vehicle: 0.5 # [m] + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: true - lower_distance_for_polygon_expansion: 30.0 # [m] - upper_distance_for_polygon_expansion: 100.0 # [m] + lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER + upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER # For target object filtering target_filtering: # avoidance target type target_type: - car: true # [-] - truck: true # [-] - bus: true # [-] - trailer: true # [-] - unknown: true # [-] - bicycle: true # [-] - motorcycle: true # [-] - pedestrian: true # [-] + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] - object_check_return_pose_distance: 20.0 # [m] - # filtering parking objects - threshold_distance_object_is_on_center: 1.0 # [m] - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # lost object compensation - object_last_seen_threshold: 2.0 + max_compensation_time: 2.0 # detection area generation parameters detection_area: - static: false # [-] - min_forward_distance: 50.0 # [m] - max_forward_distance: 150.0 # [m] - backward_distance: 10.0 # [m] + static: false # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + + # filtering parking objects + parked_vehicle: + th_offset_from_centerline: 1.0 # [m] + th_shiftable_ratio: 0.8 # [-] + min_road_shoulder_width: 0.5 # [m] FOR DEVELOPER # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: false # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + enable: true # [-] + closest_distance_to_wait_and_see: 10.0 # [m] condition: - time_threshold: 1.0 # [s] - distance_threshold: 1.0 # [m] + th_stopped_time: 3.0 # [s] + th_moving_distance: 1.0 # [m] ignore_area: traffic_light: - front_distance: 100.0 # [m] + front_distance: 100.0 # [m] crosswalk: - front_distance: 30.0 # [m] - behind_distance: 30.0 # [m] + front_distance: 30.0 # [m] + behind_distance: 30.0 # [m] # params for filtering objects that are in intersection intersection: - yaw_deviation: 0.349 # [rad] (default 20.0deg) + yaw_deviation: 0.349 # [rad] (default 20.0deg) # For safety check safety_check: # safety check target type target_type: - car: true # [-] - truck: true # [-] - bus: true # [-] - trailer: true # [-] - unknown: false # [-] - bicycle: true # [-] - motorcycle: true # [-] - pedestrian: true # [-] + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: false # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # safety check configuration - enable: true # [-] - check_current_lane: false # [-] - check_shift_side_lane: true # [-] - check_other_side_lane: false # [-] - check_unavoidable_object: false # [-] - check_other_object: true # [-] + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] # collision check parameters - check_all_predicted_path: false # [-] - safety_check_backward_distance: 100.0 # [m] - hysteresis_factor_expand_rate: 1.5 # [-] - hysteresis_factor_safe_count: 10 # [-] + check_all_predicted_path: false # [-] + safety_check_backward_distance: 100.0 # [m] + hysteresis_factor_expand_rate: 1.5 # [-] + hysteresis_factor_safe_count: 3 # [-] # predicted path parameters - min_velocity: 1.38 # [m/s] - max_velocity: 50.0 # [m/s] - time_resolution: 0.5 # [s] - time_horizon_for_front_object: 3.0 # [s] - time_horizon_for_rear_object: 10.0 # [s] - delay_until_departure: 0.0 # [s] + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 3.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] # rss parameters - extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" - expected_front_deceleration: -1.0 # [m/ss] - expected_rear_deceleration: -1.0 # [m/ss] - rear_vehicle_reaction_time: 2.0 # [s] - rear_vehicle_safety_time_margin: 1.0 # [s] - lateral_distance_max_threshold: 0.75 # [m] - longitudinal_distance_min_threshold: 3.0 # [m] - longitudinal_velocity_delta_time: 0.8 # [s] + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.0 # [s] # For avoidance maneuver avoidance: # avoidance lateral parameters lateral: - lateral_execution_threshold: 0.09 # [m] - lateral_small_shift_threshold: 0.101 # [m] - lateral_avoid_check_threshold: 0.1 # [m] - soft_road_shoulder_margin: 0.3 # [m] - hard_road_shoulder_margin: 0.3 # [m] - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - max_deviation_from_lane: 0.5 # [m] + th_avoid_execution: 0.09 # [m] FOR DEVELOPER + th_small_shift_length: 0.101 # [m] FOR DEVELOPER + soft_drivable_bound_margin: 0.3 # [m] + hard_drivable_bound_margin: 0.3 # [m] + max_right_shift_length: 5.0 # [m] FOR DEVELOPER + max_left_shift_length: 5.0 # [m] FOR DEVELOPER + max_deviation_from_lane: 0.2 # [m] # approve the next shift line after reaching this percentage of the current shift line length. - # this parameter should be within range of 0.0 to 1.0. # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. - # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.) + # this feature can be disabled by setting this parameter to 0.0. ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: @@ -232,17 +212,23 @@ max_prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] - buf_slow_down_speed: 0.56 # [m/s] - nominal_avoidance_speed: 8.33 # [m/s] + buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER + nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER + consider_front_overhang: true # [-] + consider_rear_overhang: true # [-] # return dead line return_dead_line: goal: enable: true # [-] - buffer: 30.0 # [m] + buffer: 3.0 # [m] traffic_light: enable: true # [-] buffer: 3.0 # [m] + # For cancel maneuver + cancel: + enable: true # [-] + # For yield maneuver yield: enable: true # [-] @@ -251,7 +237,7 @@ # For stop maneuver stop: max_distance: 20.0 # [m] - stop_buffer: 1.0 # [m] + stop_buffer: 1.0 # [m] FOR DEVELOPER policy: # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver". @@ -273,6 +259,10 @@ # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. use_shorten_margin_immediately: true # [-] + # -------------------------------------- + # FOLLOWING PARAMETERS ARE FOR DEVELOPER + # -------------------------------------- + constraints: # lateral constraints lateral: @@ -287,12 +277,17 @@ nominal_jerk: 0.5 # [m/sss] max_deceleration: -1.5 # [m/ss] max_jerk: 1.0 # [m/sss] - max_acceleration: 1.0 # [m/ss] + max_acceleration: 0.5 # [m/ss] + min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] shift_line_pipeline: trim: - quantize_filter_threshold: 0.1 - same_grad_filter_1_threshold: 0.1 - same_grad_filter_2_threshold: 0.2 - same_grad_filter_3_threshold: 0.5 - sharp_shift_filter_threshold: 0.2 + quantize_size: 0.1 + th_similar_grad_1: 0.1 + th_similar_grad_2: 0.2 + th_similar_grad_3: 0.5 + + # for debug + debug: + marker: false + console: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 686e4023bc7c2..e74b546f31fc4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -75,9 +75,7 @@ struct ObjectParameter double lateral_hard_margin_for_parked_vehicle{1.0}; - double safety_buffer_longitudinal{0.0}; - - bool use_conservative_buffer_longitudinal{true}; + double longitudinal_margin{0.0}; }; struct AvoidanceParameters @@ -143,6 +141,9 @@ struct AvoidanceParameters // To prevent large acceleration while avoidance. double max_acceleration{0.0}; + // To prevent large acceleration while avoidance. + double min_velocity_to_limit_max_acceleration{0.0}; + // upper distance for envelope polygon expansion. double upper_distance_for_polygon_expansion{0.0}; @@ -188,14 +189,6 @@ struct AvoidanceParameters double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; - // when complete avoidance motion, there is a distance margin with the object - // for longitudinal direction - double longitudinal_collision_margin_min_distance{0.0}; - - // when complete avoidance motion, there is a time margin with the object - // for longitudinal direction - double longitudinal_collision_margin_time{0.0}; - // parameters for safety check area bool enable_safety_check{false}; bool check_current_lane{false}; @@ -216,6 +209,9 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + bool consider_front_overhang{true}; + bool consider_rear_overhang{true}; + // maximum stop distance double stop_max_distance{0.0}; @@ -240,11 +236,11 @@ struct AvoidanceParameters // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. - double soft_road_shoulder_margin{1.0}; + double soft_drivable_bound_margin{1.0}; // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. - double hard_road_shoulder_margin{1.0}; + double hard_drivable_bound_margin{1.0}; // Even if the obstacle is very large, it will not avoid more than this length for right direction double max_right_shift_length{0.0}; @@ -273,26 +269,20 @@ struct AvoidanceParameters // line. double lateral_small_shift_threshold{0.0}; - // use for judge if the ego is shifting or not. - double lateral_avoid_check_threshold{0.0}; - // use for return shift approval. double ratio_for_return_shift_approval{0.0}; // For shift line generation process. The continuous shift length is quantized by this value. - double quantize_filter_threshold{0.0}; + double quantize_size{0.0}; // For shift line generation process. Merge small shift lines. (First step) - double same_grad_filter_1_threshold{0.0}; + double th_similar_grad_1{0.0}; // For shift line generation process. Merge small shift lines. (Second step) - double same_grad_filter_2_threshold{0.0}; + double th_similar_grad_2{0.0}; // For shift line generation process. Merge small shift lines. (Third step) - double same_grad_filter_3_threshold{0.0}; - - // For shift line generation process. Remove sharp(=jerky) shift line. - double sharp_shift_filter_threshold{0.0}; + double th_similar_grad_3{0.0}; // policy bool use_shorten_margin_immediately{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 6d2eb81b328f4..8d95e724045ba 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace behavior_path_planner::helper::avoidance @@ -32,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::PlannerData; using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; +using tier4_autoware_utils::getPose; class AvoidanceHelper { @@ -61,6 +65,11 @@ class AvoidanceHelper geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } + geometry_msgs::msg::Point getEgoPosition() const + { + return data_->self_odometry->pose.pose.position; + } + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( @@ -139,18 +148,20 @@ class AvoidanceHelper { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + if (!parameters_->consider_front_overhang) { + return object_parameter.longitudinal_margin; + } + return object_parameter.longitudinal_margin + data_->parameters.base_link2front; } double getRearConstantDistance(const ObjectData & object) const { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear + - object.length; + if (!parameters_->consider_rear_overhang) { + return object_parameter.longitudinal_margin; + } + return object_parameter.longitudinal_margin + data_->parameters.base_link2rear + object.length; } double getEgoShift() const @@ -262,6 +273,20 @@ class AvoidanceHelper return *itr; } + std::pair getDistanceToAccelEndPoint(const PathWithLaneId & path) + { + updateAccelEndPoint(path); + + if (!max_v_point_.has_value()) { + return std::make_pair(0.0, std::numeric_limits::max()); + } + + const auto start_idx = data_->findEgoIndex(path.points); + const auto distance = + calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position); + return std::make_pair(distance, max_v_point_.value().second); + } + double getFeasibleDecelDistance( const double target_velocity, const bool use_hard_constraints = true) const { @@ -347,7 +372,7 @@ class AvoidanceHelper bool isShifted() const { - return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; + return std::abs(getEgoShift()) > parameters_->lateral_execution_threshold; } bool isInitialized() const @@ -367,6 +392,56 @@ class AvoidanceHelper return true; } + void updateAccelEndPoint(const PathWithLaneId & path) + { + const auto & p = parameters_; + const auto & a_now = data_->self_acceleration->accel.accel.linear.x; + if (a_now < 0.0) { + max_v_point_ = std::nullopt; + return; + } + + if (getEgoSpeed() < p->min_velocity_to_limit_max_acceleration) { + max_v_point_ = std::nullopt; + return; + } + + if (max_v_point_.has_value()) { + return; + } + + const auto v0 = getEgoSpeed() + p->buf_slow_down_speed; + + const auto t_neg_jerk = std::max(0.0, a_now - p->max_acceleration) / p->max_jerk; + const auto v_neg_jerk = v0 + a_now * t_neg_jerk + std::pow(t_neg_jerk, 2.0) / 2.0; + const auto x_neg_jerk = v0 * t_neg_jerk + a_now * std::pow(t_neg_jerk, 2.0) / 2.0 + + p->max_jerk * std::pow(t_neg_jerk, 3.0) / 6.0; + + const auto & v_max = data_->parameters.max_vel; + if (v_max < v_neg_jerk) { + max_v_point_ = std::nullopt; + return; + } + + const auto t_max_accel = (v_max - v_neg_jerk) / p->max_acceleration; + const auto x_max_accel = + v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0; + + const auto point = + calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel); + if (point.has_value()) { + max_v_point_ = std::make_pair(point.value(), v_max); + return; + } + + const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); + const auto t_end = + (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; + const auto v_end = v0 + p->max_acceleration * t_end; + + max_v_point_ = std::make_pair(getPose(path.points.back()), v_end); + } + void reset() { prev_reference_path_ = PathWithLaneId(); @@ -417,6 +492,8 @@ class AvoidanceHelper ShiftedPath prev_linear_shift_path_; lanelet::ConstLanelets prev_driving_lanes_; + + std::optional> max_v_point_; }; } // namespace behavior_path_planner::helper::avoidance diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index abd0c017a6483..6dbf52c0425fb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -40,10 +40,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.resample_interval_for_output = getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); } // drivable area @@ -61,11 +58,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); @@ -75,10 +69,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); param.lateral_hard_margin_for_parked_vehicle = getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); - param.safety_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); - param.use_conservative_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); + param.longitudinal_margin = getOrDeclareParameter(*node, ns + "longitudinal_margin"); return param; }; @@ -124,16 +115,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.object_check_return_pose_distance = getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); p.object_check_yaw_deviation = getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "max_compensation_time"); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); } { @@ -142,9 +137,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); p.time_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.time_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.distance_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = @@ -244,16 +239,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // avoidance maneuver (lateral) { const std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = - getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); + p.soft_drivable_bound_margin = + getOrDeclareParameter(*node, ns + "soft_drivable_bound_margin"); + p.hard_drivable_bound_margin = + getOrDeclareParameter(*node, ns + "hard_drivable_bound_margin"); + p.lateral_execution_threshold = getOrDeclareParameter(*node, ns + "th_avoid_execution"); p.lateral_small_shift_threshold = - getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); - p.lateral_avoid_check_threshold = - getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + getOrDeclareParameter(*node, ns + "th_small_shift_length"); p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); p.max_deviation_from_lane = @@ -276,6 +268,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + p.consider_front_overhang = getOrDeclareParameter(*node, ns + "consider_front_overhang"); + p.consider_rear_overhang = getOrDeclareParameter(*node, ns + "consider_rear_overhang"); } // avoidance maneuver (return shift dead line) @@ -289,6 +283,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "traffic_light.buffer"); } + // cancel + { + const std::string ns = "avoidance.cancel."; + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable"); + } + // yield { const std::string ns = "avoidance.yield."; @@ -313,6 +313,10 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_shorten_margin_immediately = getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + if (p.policy_approval != "per_shift_line" && p.policy_approval != "per_avoidance_maneuver") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); } @@ -330,6 +334,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + p.min_velocity_to_limit_max_acceleration = + getOrDeclareParameter(*node, ns + "min_velocity_to_limit_max_acceleration"); } // constraints (lateral) @@ -363,17 +369,19 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // shift line pipeline { const std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); - p.same_grad_filter_1_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); - p.same_grad_filter_2_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); - p.same_grad_filter_3_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); - p.sharp_shift_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); + p.quantize_size = getOrDeclareParameter(*node, ns + "trim.quantize_size"); + p.th_similar_grad_1 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_1"); + p.th_similar_grad_2 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_2"); + p.th_similar_grad_3 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_3"); } + + // debug + { + const std::string ns = "avoidance.debug."; + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "console"); + } + return p; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 5125191a6e544..899ec99cb0d3b 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -233,6 +233,12 @@ class AvoidanceModule : public SceneModuleInterface */ void insertPrepareVelocity(ShiftedPath & shifted_path) const; + /** + * @brief insert max velocity in output path to limit acceleration. + * @param target path. + */ + void insertAvoidanceVelocity(ShiftedPath & shifted_path) const; + /** * @brief calculate stop distance based on object's overhang. * @param stop distance. diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 9dc23ea4ec80e..3f1d3495e51cb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -85,10 +85,6 @@ lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset); - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json new file mode 100644 index 0000000000000..79882beb805f8 --- /dev/null +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -0,0 +1,1437 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for behavior_path_avoidance_module", + "type": "object", + "definitions": { + "behavior_path_avoidance_module": { + "type": "object", + "properties": { + "resample_interval_for_planning": { + "type": "number", + "description": "Path resample interval for avoidance planning path.", + "default": "0.3" + }, + "resample_interval_for_output": { + "type": "number", + "description": "Path resample interval for output path. Too short interval increases computational cost for latter modules.", + "default": "4.0" + }, + "enable_bound_clipping": { + "type": "boolean", + "description": "Enable clipping left and right bound of drivable area when obstacles are in the drivable area.", + "default": "false" + }, + "disable_path_update": { + "type": "boolean", + "description": "Disable path update.", + "default": "false" + }, + "use_adjacent_lane": { + "type": "boolean", + "description": "Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.", + "default": "true" + }, + "use_opposite_lane": { + "type": "boolean", + "description": "Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects.", + "default": "true" + }, + "use_hatched_road_markings": { + "type": "boolean", + "description": "Extend drivable to hatched road marking area.", + "default": "true" + }, + "use_intersection_areas": { + "type": "boolean", + "description": "Extend drivable to intersection area.", + "default": "true" + }, + "use_freespace_areas": { + "type": "boolean", + "description": "Extend drivable to freespace area.", + "default": "true" + }, + "target_object": { + "type": "object", + "properties": { + "car": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "truck": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "bus": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "trailer": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "unknown": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": -0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": -0.2 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.1 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "motorcycle": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "bicycle": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "pedestrian": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "lower_distance_for_polygon_expansion": { + "type": "number", + "description": "If the distance between the ego and object is less than this, the expand ratio will be zero.", + "default": 30.0 + }, + "upper_distance_for_polygon_expansion": { + "type": "number", + "description": "If the distance between the ego and object is larger than this, the expand ratio will be max_expand_ratio.", + "default": 100.0 + } + }, + "required": [ + "car", + "truck", + "bus", + "trailer", + "unknown", + "bicycle", + "motorcycle", + "pedestrian", + "lower_distance_for_polygon_expansion", + "upper_distance_for_polygon_expansion" + ], + "additionalProperties": false + }, + "target_filtering": { + "type": "object", + "properties": { + "target_type": { + "type": "object", + "properties": { + "car": { + "type": "boolean", + "description": "Enable avoidance maneuver for CAR.", + "default": "true" + }, + "truck": { + "type": "boolean", + "description": "Enable avoidance maneuver for TRUCK.", + "default": "true" + }, + "bus": { + "type": "boolean", + "description": "Enable avoidance maneuver for BUS.", + "default": "true" + }, + "trailer": { + "type": "boolean", + "description": "Enable avoidance maneuver for TRAILER.", + "default": "true" + }, + "unknown": { + "type": "boolean", + "description": "Enable avoidance maneuver for UNKNOWN.", + "default": "true" + }, + "bicycle": { + "type": "boolean", + "description": "Enable avoidance maneuver for BICYCLE.", + "default": "true" + }, + "motorcycle": { + "type": "boolean", + "description": "Enable avoidance maneuver for MOTORCYCLE.", + "default": "true" + }, + "pedestrian": { + "type": "boolean", + "description": "Enable avoidance maneuver for PEDESTRIAN.", + "default": "true" + } + }, + "required": [ + "car", + "truck", + "bus", + "trailer", + "unknown", + "bicycle", + "motorcycle", + "pedestrian" + ], + "additionalProperties": false + }, + "object_check_goal_distance": { + "type": "number", + "description": "If the distance between object and goal position is less than this parameter, the module do not return center line.", + "default": 20.0 + }, + "object_check_return_pose_distance": { + "type": "number", + "description": "If the distance between object and return position is less than this parameter, the module do not return center line.", + "default": 20.0 + }, + "max_compensation_time": { + "type": "number", + "description": "For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit.", + "default": 2.0 + }, + "detection_area": { + "type": "object", + "properties": { + "static": { + "type": "boolean", + "description": "If true, the detection area longitudinal range is calculated based on current ego speed.", + "default": "false" + }, + "min_forward_distance": { + "type": "number", + "description": "Minimum forward distance to search the avoidance target.", + "default": 50.0 + }, + "max_forward_distance": { + "type": "number", + "description": "Maximum forward distance to search the avoidance target.", + "default": 150.0 + }, + "backward_distance": { + "type": "number", + "description": "Backward distance to search the avoidance target.", + "default": 10.0 + } + }, + "required": [ + "static", + "min_forward_distance", + "max_forward_distance", + "backward_distance" + ], + "additionalProperties": false + }, + "parked_vehicle": { + "type": "object", + "properties": { + "th_offset_from_centerline": { + "type": "number", + "description": "Vehicles around the center line within this distance will be excluded from avoidance target.", + "default": 1.0 + }, + "th_shiftable_ratio": { + "type": "number", + "description": "Vehicles around the center line within this distance will be excluded from avoidance target.", + "default": 0.8, + "minimum": 0.0, + "maximum": 1.0 + }, + "min_road_shoulder_width": { + "type": "number", + "description": "Width considered as a road shoulder if the lane does not have a road shoulder target.", + "default": 0.5 + } + }, + "required": [ + "th_offset_from_centerline", + "th_shiftable_ratio", + "min_road_shoulder_width" + ], + "additionalProperties": false + }, + "avoidance_for_ambiguous_vehicle": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Enable avoidance maneuver for ambiguous vehicles.", + "default": "true" + }, + "closest_distance_to_wait_and_see": { + "type": "number", + "description": "Start avoidance maneuver after the distance to ambiguous vehicle is less than this param.", + "default": 10.0 + }, + "condition": { + "type": "object", + "properties": { + "th_stopped_time": { + "type": "number", + "description": "Never avoid object whose stopped time is less than this param.", + "default": 3.0 + }, + "th_moving_distance": { + "type": "number", + "description": "Never avoid object which moves more than this param.", + "default": 1.0 + } + }, + "required": ["th_stopped_time", "th_moving_distance"], + "additionalProperties": false + }, + "ignore_area": { + "type": "object", + "properties": { + "traffic_light": { + "type": "object", + "properties": { + "front_distance": { + "type": "number", + "description": "If the distance between traffic light and vehicle is less than this parameter, this module will ignore it.", + "default": 100.0 + } + }, + "required": ["front_distance"], + "additionalProperties": false + }, + "crosswalk": { + "type": "object", + "properties": { + "front_distance": { + "type": "number", + "description": "If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it.", + "default": 30.0 + }, + "behind_distance": { + "type": "number", + "description": "If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it.", + "default": 30.0 + } + }, + "required": ["front_distance", "behind_distance"], + "additionalProperties": false + } + }, + "required": ["traffic_light", "crosswalk"], + "additionalProperties": false + } + }, + "required": [ + "enable", + "closest_distance_to_wait_and_see", + "condition", + "ignore_area" + ], + "additionalProperties": false + }, + "intersection": { + "type": "object", + "properties": { + "yaw_deviation": { + "type": "number", + "description": "Yaw deviation threshold param to judge if the object is not merging or deviating vehicle.", + "default": 0.349 + } + }, + "required": ["yaw_deviation"], + "additionalProperties": false + } + }, + "required": [ + "target_type", + "object_check_goal_distance", + "object_check_return_pose_distance", + "max_compensation_time", + "detection_area", + "parked_vehicle", + "avoidance_for_ambiguous_vehicle", + "intersection" + ], + "additionalProperties": false + }, + "safety_check": { + "type": "object", + "properties": { + "target_type": { + "type": "object", + "properties": { + "car": { + "type": "boolean", + "description": "Enable safety_check for CAR.", + "default": "true" + }, + "truck": { + "type": "boolean", + "description": "Enable safety_check for TRUCK.", + "default": "true" + }, + "bus": { + "type": "boolean", + "description": "Enable safety_check for BUS.", + "default": "true" + }, + "trailer": { + "type": "boolean", + "description": "Enable safety_check for TRAILER.", + "default": "true" + }, + "unknown": { + "type": "boolean", + "description": "Enable safety_check for UNKNOWN.", + "default": "false" + }, + "bicycle": { + "type": "boolean", + "description": "Enable safety_check for BICYCLE.", + "default": "true" + }, + "motorcycle": { + "type": "boolean", + "description": "Enable safety_check for MOTORCYCLE.", + "default": "true" + }, + "pedestrian": { + "type": "boolean", + "description": "Enable safety_check for PEDESTRIAN.", + "default": "true" + } + }, + "required": [ + "car", + "truck", + "bus", + "trailer", + "unknown", + "bicycle", + "motorcycle", + "pedestrian" + ], + "additionalProperties": false + }, + "enable": { + "type": "boolean", + "description": "Enable to use safety check feature.", + "default": "true" + }, + "check_current_lane": { + "type": "boolean", + "description": "Check objects on current driving lane.", + "default": "true" + }, + "check_shift_side_lane": { + "type": "boolean", + "description": "Check objects on shift side lane.", + "default": "true" + }, + "check_other_side_lane": { + "type": "boolean", + "description": "Check objects on other side lane.", + "default": "true" + }, + "check_unavoidable_object": { + "type": "boolean", + "description": "Check collision between ego and unavoidable objects.", + "default": "true" + }, + "check_other_object": { + "type": "boolean", + "description": "Check collision between ego and non avoidance target objects.", + "default": "true" + }, + "check_all_predicted_path": { + "type": "boolean", + "description": "Check all prediction path of safety check target objects.", + "default": "true" + }, + "safety_check_backward_distance": { + "type": "number", + "description": "Backward distance to search the dynamic objects.", + "default": 100.0 + }, + "hysteresis_factor_expand_rate": { + "type": "number", + "description": "Hysteresis factor that be used for chattering prevention.", + "default": 2.0 + }, + "hysteresis_factor_safe_count": { + "type": "integer", + "description": "Hysteresis count that be used for chattering prevention.", + "default": 10 + }, + "min_velocity": { + "type": "number", + "description": "Minimum velocity of the ego vehicle's predicted path.", + "default": 1.38 + }, + "max_velocity": { + "type": "number", + "description": "Maximum velocity of the ego vehicle's predicted path.", + "default": 50.0 + }, + "time_resolution": { + "type": "number", + "description": "Time resolution for the ego vehicle's predicted path.", + "default": 0.5 + }, + "time_horizon_for_front_object": { + "type": "number", + "description": "Time horizon for predicting front objects.", + "default": 3.0 + }, + "time_horizon_for_rear_object": { + "type": "number", + "description": "Time horizon for predicting rear objects.", + "default": 10.0 + }, + "delay_until_departure": { + "type": "number", + "description": "Delay until the ego vehicle departs.", + "default": 1.0 + }, + "extended_polygon_policy": { + "type": "string", + "enum": ["rectangle", "along_path"], + "description": "See https://github.com/autowarefoundation/autoware.universe/pull/6336.", + "default": "along_path" + }, + "expected_front_deceleration": { + "type": "number", + "description": "The front object's maximum deceleration when the front vehicle perform sudden braking.", + "default": -1.0 + }, + "expected_rear_deceleration": { + "type": "number", + "description": "The rear object's maximum deceleration when the rear vehicle perform sudden braking.", + "default": -1.0 + }, + "rear_vehicle_reaction_time": { + "type": "number", + "description": "The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake.", + "default": 2.0 + }, + "rear_vehicle_safety_time_margin": { + "type": "number", + "description": "The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking.", + "default": 1.0 + }, + "lateral_distance_max_threshold": { + "type": "number", + "description": "The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.", + "default": 2.0 + }, + "longitudinal_distance_min_threshold": { + "type": "number", + "description": "The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.", + "default": 3.0 + }, + "longitudinal_velocity_delta_time": { + "type": "number", + "description": "The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)", + "default": 0.0 + } + }, + "required": [ + "target_type", + "enable", + "check_current_lane", + "check_shift_side_lane", + "check_other_side_lane", + "check_unavoidable_object", + "check_other_object", + "check_all_predicted_path", + "safety_check_backward_distance", + "hysteresis_factor_expand_rate", + "hysteresis_factor_safe_count", + "min_velocity", + "max_velocity", + "time_resolution", + "time_horizon_for_front_object", + "time_horizon_for_rear_object", + "delay_until_departure", + "extended_polygon_policy", + "expected_front_deceleration", + "expected_rear_deceleration", + "rear_vehicle_reaction_time", + "rear_vehicle_safety_time_margin", + "lateral_distance_max_threshold", + "longitudinal_distance_min_threshold", + "longitudinal_velocity_delta_time" + ], + "additionalProperties": false + }, + "avoidance": { + "type": "object", + "properties": { + "lateral": { + "type": "object", + "properties": { + "th_avoid_execution": { + "type": "number", + "description": "The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance.", + "default": 0.09 + }, + "th_small_shift_length": { + "type": "number", + "description": "The shift lines whose lateral offset is less than this will be applied with other ones.", + "default": 0.101 + }, + "soft_drivable_bound_margin": { + "type": "number", + "description": "Keep distance to drivable bound.", + "default": 0.3 + }, + "hard_drivable_bound_margin": { + "type": "number", + "description": "Keep distance to drivable bound.", + "default": 0.3 + }, + "max_right_shift_length": { + "type": "number", + "description": "Maximum shift length for right direction", + "default": 5.0 + }, + "max_left_shift_length": { + "type": "number", + "description": "Maximum shift length for left direction.", + "default": 5.0 + }, + "max_deviation_from_lane": { + "type": "number", + "description": "Use in validation phase to check if the avoidance path is in drivable area.", + "default": 0.2 + }, + "ratio_for_return_shift_approval": { + "type": "number", + "description": "This parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + } + }, + "required": [ + "th_avoid_execution", + "th_small_shift_length", + "soft_drivable_bound_margin", + "hard_drivable_bound_margin", + "max_deviation_from_lane", + "ratio_for_return_shift_approval" + ], + "additionalProperties": false + }, + "longitudinal": { + "type": "object", + "properties": { + "min_prepare_time": { + "type": "number", + "description": "Avoidance shift starts from point ahead of this time x ego speed at least.", + "default": 1.0 + }, + "max_prepare_time": { + "type": "number", + "description": "Avoidance shift starts from point ahead of this time x ego speed if possible.", + "default": 2.0 + }, + "min_prepare_distance": { + "type": "number", + "description": "Minimum prepare distance.", + "default": 1.0 + }, + "min_slow_down_speed": { + "type": "number", + "description": "Minimum slow speed for avoidance prepare section.", + "default": 1.38 + }, + "buf_slow_down_speed": { + "type": "number", + "description": "Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed.", + "default": 0.56 + }, + "nominal_avoidance_speed": { + "type": "number", + "description": "Nominal avoidance speed.", + "default": 8.33 + }, + "consider_front_overhang": { + "type": "boolean", + "description": "Flag to consider vehicle front overhang in shift line generation logic.", + "default": true + }, + "consider_rear_overhang": { + "type": "boolean", + "description": "Flag to consider vehicle rear overhang in shift line generation logic.", + "default": true + } + }, + "required": [ + "min_prepare_time", + "max_prepare_time", + "min_prepare_distance", + "min_slow_down_speed", + "buf_slow_down_speed", + "nominal_avoidance_speed", + "consider_front_overhang", + "consider_rear_overhang" + ], + "additionalProperties": false + }, + "return_dead_line": { + "type": "object", + "properties": { + "goal": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Insert stop point in order to return original lane before reaching goal.", + "default": "true" + }, + "buffer": { + "type": "number", + "description": "Buffer distance to return original lane before reaching goal.", + "default": 3.0 + } + }, + "required": ["enable", "buffer"], + "additionalProperties": false + }, + "traffic_light": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Insert stop point in order to return original lane before reaching traffic light.", + "default": "true" + }, + "buffer": { + "type": "number", + "description": "Buffer distance to return original lane before reaching traffic light.", + "default": 3.0 + } + }, + "required": ["enable", "buffer"], + "additionalProperties": false + } + }, + "required": ["goal", "traffic_light"], + "additionalProperties": false + } + }, + "required": ["lateral", "longitudinal", "return_dead_line"], + "additionalProperties": false + }, + "stop": { + "type": "object", + "properties": { + "max_distance": { + "type": "number", + "description": "Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver.", + "default": 20.0 + }, + "stop_buffer": { + "type": "number", + "description": "Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver.", + "default": 1.0 + } + }, + "required": ["max_distance", "stop_buffer"], + "additionalProperties": false + }, + "yield": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Flag to enable yield maneuver.", + "default": "true" + }, + "enable_during_shifting": { + "type": "boolean", + "description": "Flag to enable yield maneuver during shifting.", + "default": "false" + } + }, + "required": ["enable", "enable_during_shifting"], + "additionalProperties": false + }, + "cancel": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Flag to enable cancel maneuver.", + "default": "true" + } + }, + "required": ["enable"], + "additionalProperties": false + }, + "policy": { + "type": "object", + "properties": { + "make_approval_request": { + "type": "string", + "enum": ["per_shift_line", "per_avoidance_maneuver"], + "description": "policy for rtc request. select `per_shift_line` or `per_avoidance_maneuver`. `per_shift_line`: request approval for each shift line. `per_avoidance_maneuver`: request approval for avoidance maneuver (avoid + return).", + "default": "per_shift_line" + }, + "deceleration": { + "type": "string", + "enum": ["reliable", "best_effort"], + "description": "policy for vehicle slow down behavior. select `best_effort` or `reliable`. `best_effort`: slow down deceleration & jerk are limited by constraints but there is a possibility that the vehicle can't stop in front of the vehicle. `reliable`: insert stop or slow down point with ignoring decel/jerk constraints. make it possible to increase chance to avoid but uncomfortable deceleration maybe happen.", + "default": "best_effort" + }, + "lateral_margin": { + "type": "string", + "enum": ["reliable", "best_effort"], + "description": "policy for voidance lateral margin. select `best_effort` or `reliable`. `best_effort`: output avoidance path with shorten lateral margin when there is no enough longitudinal margin to avoid. `reliable`: module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid with expected lateral margin.", + "default": "best_effort" + }, + "use_shorten_margin_immediately": { + "type": "boolean", + "description": "if true, module doesn't wait deceleration and outputs avoidance path by best effort margin.", + "default": "true" + } + }, + "required": ["make_approval_request"], + "additionalProperties": false + }, + "constraints": { + "type": "object", + "properties": { + "lateral": { + "type": "object", + "properties": { + "velocity": { + "type": "array", + "description": "Velocity array to decide current lateral accel/jerk limit.", + "default": [1.0, 1.38, 11.1] + }, + "max_accel_values": { + "type": "array", + "description": "Avoidance path gets sharp up to this accel limit when there is not enough distance from ego.", + "default": [0.5, 0.5, 0.5] + }, + "min_jerk_values": { + "type": "array", + "description": "Avoidance path is generated with this jerk when there is enough distance from ego.", + "default": [0.2, 0.2, 0.2] + }, + "max_jerk_values": { + "type": "array", + "description": "Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego.", + "default": [1.0, 1.0, 1.0] + } + }, + "required": ["velocity", "max_accel_values", "min_jerk_values", "max_jerk_values"], + "additionalProperties": false + }, + "longitudinal": { + "type": "object", + "properties": { + "nominal_deceleration": { + "type": "number", + "description": "Nominal deceleration limit.", + "default": -1.0 + }, + "nominal_jerk": { + "type": "number", + "description": "Nominal jerk limit.", + "default": 0.5 + }, + "max_deceleration": { + "type": "number", + "description": "Max deceleration limit.", + "default": -1.5 + }, + "max_jerk": { + "type": "number", + "description": "Max jerk limit.", + "default": 1.0 + }, + "max_acceleration": { + "type": "number", + "description": "Maximum acceleration during avoidance.", + "default": 0.5 + }, + "min_velocity_to_limit_max_acceleration": { + "type": "number", + "description": "If the ego speed is faster than this param, the module applies acceleration limit `max_acceleration`.", + "default": 2.78 + } + }, + "required": [ + "nominal_deceleration", + "nominal_jerk", + "max_deceleration", + "max_jerk", + "max_acceleration", + "min_velocity_to_limit_max_acceleration" + ], + "additionalProperties": false + } + }, + "required": ["lateral", "longitudinal"], + "additionalProperties": false + }, + "shift_line_pipeline": { + "type": "object", + "properties": { + "trim": { + "type": "object", + "properties": { + "quantize_size": { + "type": "number", + "description": "Lateral shift length quantize size.", + "default": 0.1 + }, + "th_similar_grad_1": { + "type": "number", + "description": "Lateral shift length threshold to merge similar gradient shift lines.", + "default": 0.1 + }, + "th_similar_grad_2": { + "type": "number", + "description": "Lateral shift length threshold to merge similar gradient shift lines.", + "default": 0.2 + }, + "th_similar_grad_3": { + "type": "number", + "description": "Lateral shift length threshold to merge similar gradient shift lines.", + "default": 0.5 + } + }, + "required": [ + "quantize_size", + "th_similar_grad_1", + "th_similar_grad_2", + "th_similar_grad_3" + ], + "additionalProperties": false + } + }, + "required": ["trim"], + "additionalProperties": false + }, + "debug": { + "type": "object", + "properties": { + "marker": { + "type": "boolean", + "description": "Publish debug marker.", + "default": "false" + }, + "console": { + "type": "boolean", + "description": "Output debug info on console.", + "default": "false" + } + }, + "required": ["marker", "console"], + "additionalProperties": false + } + }, + "required": [ + "resample_interval_for_planning", + "resample_interval_for_output", + "enable_bound_clipping", + "use_adjacent_lane", + "use_opposite_lane", + "use_hatched_road_markings", + "use_intersection_areas", + "use_freespace_areas", + "target_object", + "target_filtering", + "safety_check", + "avoidance", + "stop", + "yield", + "cancel", + "policy", + "constraints", + "shift_line_pipeline", + "debug" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "avoidance": { + "$ref": "#/definitions/behavior_path_avoidance_module" + } + }, + "required": ["avoidance"], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 692e4260520f3..e3391f251e55f 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -46,18 +46,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "enable_safety_check", p->enable_safety_check); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - updateParam(parameters, ns + "print_debug_info", p->print_debug_info); - } - const auto update_object_param = [&p, ¶meters]( const auto & semantic, const std::string & ns) { auto & config = p->object_parameters.at(semantic); - updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); - updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); + updateParam(parameters, ns + "th_moving_speed", config.moving_speed_threshold); + updateParam(parameters, ns + "th_moving_time", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); updateParam(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); @@ -65,11 +58,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector( parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", config.lateral_hard_margin_for_parked_vehicle); - updateParam( - parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); - updateParam( - parameters, ns + "use_conservative_buffer_longitudinal", - config.use_conservative_buffer_longitudinal); + updateParam(parameters, ns + "longitudinal_margin", config.longitudinal_margin); }; { @@ -92,23 +81,94 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorobject_parameters.count(object_type) == 0) { + return; + } + updateParam(parameters, ns, p->object_parameters.at(object_type).is_avoidance_target); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + updateParam( - parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold); + parameters, ns + "object_check_goal_distance", p->object_check_goal_distance); + updateParam( + parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance); + updateParam(parameters, ns + "max_compensation_time", p->object_last_seen_threshold); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; + updateParam( + parameters, ns + "th_offset_from_centerline", p->threshold_distance_object_is_on_center); + updateParam(parameters, ns + "th_shiftable_ratio", p->object_check_shiftable_ratio); + updateParam( + parameters, ns + "min_road_shoulder_width", p->object_check_min_road_shoulder_width); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + updateParam(parameters, ns + "static", p->use_static_detection_area); + updateParam( + parameters, ns + "min_forward_distance", p->object_check_min_forward_distance); + updateParam( + parameters, ns + "max_forward_distance", p->object_check_max_forward_distance); + updateParam(parameters, ns + "backward_distance", p->object_check_backward_distance); + } + + { + const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; + updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam( + parameters, ns + "closest_distance_to_wait_and_see", + p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + updateParam( + parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); + updateParam( + parameters, ns + "condition.th_moving_distance", p->distance_threshold_for_ambiguous_vehicle); + updateParam( + parameters, ns + "ignore_area.traffic_light.front_distance", + p->object_ignore_section_traffic_light_in_front_distance); + updateParam( + parameters, ns + "ignore_area.crosswalk.front_distance", + p->object_ignore_section_crosswalk_in_front_distance); + updateParam( + parameters, ns + "ignore_area.crosswalk.behind_distance", + p->object_ignore_section_crosswalk_behind_distance); + } + + { + const std::string ns = "avoidance.target_filtering.intersection."; + updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); + } + + { + const std::string ns = "avoidance.avoidance.lateral."; + updateParam(parameters, ns + "th_avoid_execution", p->lateral_execution_threshold); + updateParam(parameters, ns + "th_small_shift_length", p->lateral_small_shift_threshold); updateParam( - parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); + parameters, ns + "soft_drivable_bound_margin", p->soft_drivable_bound_margin); updateParam( - parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold); - updateParam(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin); - updateParam(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin); + parameters, ns + "hard_drivable_bound_margin", p->hard_drivable_bound_margin); } { const std::string ns = "avoidance.avoidance.longitudinal."; updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); + updateParam(parameters, ns + "min_prepare_distance", p->min_prepare_distance); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); + updateParam(parameters, ns + "consider_front_overhang", p->consider_front_overhang); + updateParam(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); } { @@ -117,6 +177,33 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_buffer", p->stop_buffer); } + { + const std::string ns = "avoidance.policy."; + updateParam(parameters, ns + "make_approval_request", p->policy_approval); + updateParam(parameters, ns + "deceleration", p->policy_deceleration); + updateParam(parameters, ns + "lateral_margin", p->policy_lateral_margin); + updateParam( + parameters, ns + "use_shorten_margin_immediately", p->use_shorten_margin_immediately); + + if (p->policy_approval != "per_shift_line" && p->policy_approval != "per_avoidance_maneuver") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid policy. please select 'per_shift_line' or 'per_avoidance_maneuver'."); + } + + if (p->policy_deceleration != "best_effort" && p->policy_deceleration != "reliable") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid deceleration policy. Please select 'best_effort' or 'reliable'."); + } + + if (p->policy_lateral_margin != "best_effort" && p->policy_lateral_margin != "reliable") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid lateral margin policy. Please select 'best_effort' or 'reliable'."); + } + } + { const std::string ns = "avoidance.constrains.lateral."; @@ -147,17 +234,30 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector( - parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); + const std::string ns = "avoidance.constraints.longitudinal."; + + updateParam(parameters, ns + "nominal_deceleration", p->nominal_deceleration); + updateParam(parameters, ns + "nominal_jerk", p->nominal_jerk); + updateParam(parameters, ns + "max_deceleration", p->max_deceleration); + updateParam(parameters, ns + "max_jerk", p->max_jerk); + updateParam(parameters, ns + "max_acceleration", p->max_acceleration); updateParam( - parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); + parameters, ns + "min_velocity_to_limit_max_acceleration", + p->min_velocity_to_limit_max_acceleration); + } + + { + const std::string ns = "avoidance.shift_line_pipeline."; + updateParam(parameters, ns + "trim.quantize_size", p->quantize_size); + updateParam(parameters, ns + "trim.th_similar_grad_1", p->th_similar_grad_1); + updateParam(parameters, ns + "trim.th_similar_grad_2", p->th_similar_grad_2); + updateParam(parameters, ns + "trim.th_similar_grad_3", p->th_similar_grad_3); + } + + { + const std::string ns = "avoidance.debug."; + updateParam(parameters, ns + "marker", p->publish_debug_marker); + updateParam(parameters, ns + "console", p->print_debug_info); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 12c59daef93ad..b16395f825385 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -162,7 +162,7 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & const bool has_shift_point = !path_shifter_.getShiftLines().empty(); const bool has_base_offset = - std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_execution_threshold; // Nothing to do. -> EXIT. if (!has_shift_point && !has_base_offset) { @@ -318,7 +318,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; - using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; using utils::traffic_light::calcDistanceToRedTrafficLight; @@ -642,7 +641,10 @@ void AvoidanceModule::fillDebugData( const auto constant_distance = helper_->getFrontConstantDistance(o_front); const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor + + const auto lateral_hard_margin = o_front.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto max_avoid_margin = lateral_hard_margin * o_front.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto avoidance_distance = helper_->getSharpAvoidanceDistance( @@ -686,6 +688,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } insertPrepareVelocity(path); + insertAvoidanceVelocity(path); switch (data.state) { case AvoidanceState::NOT_AVOID: { @@ -731,7 +734,7 @@ bool AvoidanceModule::isSafePath( for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); - if (parameters_->lateral_avoid_check_threshold < length) { + if (parameters_->lateral_execution_threshold < length) { return true; } } @@ -743,7 +746,7 @@ bool AvoidanceModule::isSafePath( for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); - if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { + if (parameters_->lateral_execution_threshold < -1.0 * length) { return true; } } @@ -1219,7 +1222,7 @@ bool AvoidanceModule::isValidShiftLine( // check if the vehicle is in road. (yaw angle is not considered) { const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + - parameters_->hard_road_shoulder_margin - + parameters_->hard_drivable_bound_margin - parameters_->max_deviation_from_lane; const size_t start_idx = shift_lines.front().start_idx; @@ -1431,7 +1434,10 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto avoid_margin = lateral_hard_margin * object.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); @@ -1675,7 +1681,10 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.value().object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.lateral_hard_margin * object.value().distance_factor + + const auto lateral_hard_margin = object.value().is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto avoid_margin = lateral_hard_margin * object.value().distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength( object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin); @@ -1725,6 +1734,52 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_object); } +void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const +{ + const auto & data = avoid_data_; + + // do nothing if no shift line is approved. + if (path_shifter_.getShiftLines().empty()) { + return; + } + + // do nothing if there is no avoidance target. + if (data.target_objects.empty()) { + return; + } + + const auto [distance_to_accel_end_point, v_max] = + helper_->getDistanceToAccelEndPoint(shifted_path.path); + if (distance_to_accel_end_point < 1e-3) { + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto accel_distance = distance_to_accel_end_point - distance_from_ego; + if (accel_distance < 0.0) { + break; + } + + const double v_target_square = + v_max * v_max - 2.0 * parameters_->max_acceleration * accel_distance; + if (v_target_square < 1e-3) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = std::max(getEgoSpeed(), std::sqrt(v_target_square)); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target); + } + + slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + shifted_path.path.points, start_idx, distance_to_accel_end_point); +} + std::shared_ptr AvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now(); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 5a8fd35f6d618..9137048945fa1 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -871,7 +871,7 @@ AvoidLineArray ShiftLineGenerator::applyTrimProcess( // - Combine avoid points that have almost same gradient. // this is to remove the noise. { - const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; + const auto THRESHOLD = parameters_->th_similar_grad_1; applySimilarGradFilter(sl_array_trimmed, THRESHOLD); debug.step3_grad_filtered_1st = sl_array_trimmed; } @@ -879,7 +879,7 @@ AvoidLineArray ShiftLineGenerator::applyTrimProcess( // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { - const auto THRESHOLD = parameters_->quantize_filter_threshold; + const auto THRESHOLD = parameters_->quantize_size; applyQuantizeProcess(sl_array_trimmed, THRESHOLD); debug.step3_quantize_filtered = sl_array_trimmed; } @@ -893,14 +893,14 @@ AvoidLineArray ShiftLineGenerator::applyTrimProcess( // - Combine avoid points that have almost same gradient (again) { - const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; + const auto THRESHOLD = parameters_->th_similar_grad_2; applySimilarGradFilter(sl_array_trimmed, THRESHOLD); debug.step3_grad_filtered_2nd = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { - const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; + const auto THRESHOLD = parameters_->th_similar_grad_3; applySimilarGradFilter(sl_array_trimmed, THRESHOLD); debug.step3_grad_filtered_3rd = sl_array_trimmed; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index fdb2b9b71c782..bef3976be7e1e 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -575,9 +575,11 @@ bool isNeverAvoidanceTarget( } if (object.is_on_ego_lane) { - if ( - planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() && - planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) { + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); + if (right_lane.has_value() && left_lane.has_value()) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); return true; @@ -812,9 +814,9 @@ std::optional getAvoidMargin( object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto min_avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; const auto soft_lateral_distance_limit = - object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; + object.to_road_shoulder_distance - parameters->soft_drivable_bound_margin - 0.5 * vehicle_width; const auto hard_lateral_distance_limit = - object.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; + object.to_road_shoulder_distance - parameters->hard_drivable_bound_margin - 0.5 * vehicle_width; // Step1. check avoidable or not. if (hard_lateral_distance_limit < min_avoid_margin) { @@ -872,8 +874,7 @@ double getRoadShoulderDistance( } { - const auto p2 = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -100.0 : 100.0), 0.0).position; + const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position; const auto opt_intersect = tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); @@ -1219,43 +1220,6 @@ std::vector generateObstaclePolygonsForDrivableArea( return obstacles_for_drivable_area; } -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset) -{ - const auto & rh = planner_data->route_handler; - - lanelet::ConstLanelets target_lanelets{}; - for (const auto & lane : route_lanelets) { - auto l_offset = 0.0; - auto r_offset = 0.0; - - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (opt_left_lane) { - target_lanelets.push_back(opt_left_lane.value()); - } else { - l_offset = left_offset; - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (opt_right_lane) { - target_lanelets.push_back(opt_right_lane.value()); - } else { - r_offset = right_offset; - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (!right_opposite_lanes.empty()) { - target_lanelets.push_back(right_opposite_lanes.front()); - } - - const auto expand_lane = lanelet::utils::getExpandedLanelet(lane, l_offset, r_offset); - target_lanelets.push_back(expand_lane); - } - - return target_lanelets; -} - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data) { diff --git a/planning/behavior_path_dynamic_avoidance_module/README.md b/planning/behavior_path_dynamic_avoidance_module/README.md index 10276db0efab2..89a661d66735e 100644 --- a/planning/behavior_path_dynamic_avoidance_module/README.md +++ b/planning/behavior_path_dynamic_avoidance_module/README.md @@ -4,15 +4,16 @@ This module is under development. ## Purpose / Role -This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the obstacle_avoidance module. +This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the [obstacle_avoidance_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/). Each module performs the following roles. -Dynamic Avoidance module: This module cuts off the drivable area according to the position and velocity of the target to be avoided. -Obstacle Avoidance module: This module modifies the path to be followed so that it fits within the drivable area received. +Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided. +Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area. -Avoidance functions are also provided by the Avoidance module, which allows avoidance through the outside of own lanes but not against moving objects. +Avoidance functions are also provided by the [Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_avoidance_module/), but these modules have different roles. +The Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. On the other hand, this module can avoid moving objects. -For this reason, the word "dynamic" is used in its name. -The table below lists the avoidance modules that can be used for each situation. +For this reason, the word "dynamic" is used in the modules's name. +The table below lists the avoidance modules that can handle each situation. | | avoid within the own lane | avoid through the outside of own lanes | | :----------------------- | :------------------------------------------------------------------------: | :------------------------------------: | @@ -23,7 +24,6 @@ The table below lists the avoidance modules that can be used for each situation. Here, we describe the policy of inner algorithms. The inner algorithms can be separated into two parts: The first decide whether to avoid the obstacles and the second cuts off the drivable area against the corresponding obstacle. -If you are interested in more details, please see the code itself. ### Select obstacles to avoid @@ -31,11 +31,10 @@ To decide whether to avoid an object, both the predicted path and the state (pos The type of objects the user wants this module to avoid is also required. Using this information, the module decides to _avoid_ objects that _obstruct the ego's passage_ and _can be avoided_. -The definition of _obstruct own passage_ is implemented as the object that collides within seconds. -This process wastes computational cost by doing it for all objects; thus, filtering by the relative position and speed of the object with respect to the ego's path is also done as an auxiliary process. -The other, _can be avoided_ denotes whether it can be avoided without risk to passengers or other vehicles. -For this purpose, it is judged whether the obstacle can be avoided by satisfying the constraints of lateral acceleration and lateral jerk. -For example, the module decides not to avoid an object that is too close or fast in the lateral direction because it cannot be avoided. +The definition of _obstruct the ego's passage_ is implemented as the object that collides within seconds. +The other, _can be avoided_ denotes whether it can be avoided without risk to the passengers or the other vehicles. +For this purpose, the module judges whether the obstacle can be avoided with satisfying the constraints of lateral acceleration and lateral jerk. +For example, the module decides not to avoid an object that is too close or fast in the lateral direction. ### Cuts off the drivable area against the selected obstacles diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 7310e93dda4bb..a890cfc21ed13 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1640,34 +1640,6 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); return new_signal; - // // calc TurnIndicatorsCommand - // { - // const double distance_to_end = - // calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); - // const bool is_before_end_pose = distance_to_end >= 0.0; - // if (is_before_end_pose) { - // if (left_side_parking_) { - // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - // } else { - // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - // } - // } else { - // turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; - // } - // } - - // // calc desired/required start/end point - // { - // // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds - // // before starting pull_over - // turn_signal.desired_start_point = - // last_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose; - // turn_signal.desired_end_point = end_pose; - // turn_signal.required_start_point = start_pose; - // turn_signal.required_end_point = end_pose; - // } - - // return turn_signal; } bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 8eb791ab4f5e9..62f35a053958a 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -4,6 +4,11 @@ The Lane Change module is activated when lane change is needed and can be safely ## Lane Change Requirement +- As the prerequisite, the type of lane boundary in the HD map has to be one of the following: + - Dashed lane marking: Lane changes are permitted in both directions. + - Dashed marking on the left and solid on the right: Lane changes are allowed from left to right. + - Dashed marking on the right and solid on the left: Lane changes are allowed from right to left. + - `allow_lane_change` tags is set as `true` - During lane change request condition - The ego-vehicle isn’t on a `preferred_lane`. - There is neither intersection nor crosswalk on the path of the lane change @@ -60,6 +65,89 @@ longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - mini Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate). +The chart illustrates the conditions under which longitudinal acceleration values are sampled. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (prev_module_path is empty?) then (yes) + :Return empty list; + stop +else (no) +endif + +if (max_acc <= 0.0) then (yes) + :Return **sampled acceleration values**; + note left: Calculated sampled acceleration using\ngetAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num) + stop +endif + +if (max_lane_change_length > ego's distance to the end of the current lanes.) then (yes) + :Return **sampled acceleration values**; + stop +endif + +if (isVehicleStuck(current_lanes)) then (yes) + :Return **sampled acceleration values**; + stop +else (no) +endif + +if (is goal is in target lanes) then (yes) + if (max_lane_change_length < ego's distance to the goal along the target lanes) then (yes) + :Return {max_acc}; + stop + else (no) + endif +else (no) + if (max_lane_change_length < ego's distance to the end of the target lanes.) then (yes) + :Return {max_acc}; + stop + else (no) + endif +endif + +:Return **sampled acceleration values**; +stop + +@enduml + +``` + +while the following describes the process by which longitudinal accelerations are sampled. + +```plantuml +@startuml +start +:Initialize sampled_values with min_acc; + +if (min_acc > max_acc) then (yes) + :Return empty list; + stop +elseif (max_acc - min_acc < epsilon) then (yes) + :Return {0.0}; + stop +else (no) + :Calculate resolution; +endif + +:Start loop from min_acc to max_acc with resolution step; +repeat + if (sampled_values.back() < -epsilon AND next_value > epsilon) then (yes) + :Insert 0.0 into sampled_values; + endif + :Add sampled_acc to sampled_values; + repeat while (sampled_acc < max_acc + epsilon) is (TRUE) + +:Return sampled_values; +stop +@enduml +``` + The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. ![path_samples](./images/lane_change-candidate_path_samples.png) @@ -89,67 +177,112 @@ lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_latera #### Candidate Path's validity check -A candidate path is valid if the total lane change distance is less than +A candidate path is considered valid if it meets the following criteria: -1. distance to the end of current lane -2. distance to the next intersection -3. distance from current pose to the goal. -4. distance to the crosswalk. - -The goal must also be in the list of the preferred lane. +1. The distance from the ego vehicle's current position to the end of the current lanes is sufficient to perform a single lane change. +2. The distance from the ego vehicle's current position to the goal along the current lanes is adequate to complete multiple lane changes. +3. The distance from the ego vehicle's current position to the end of the target lanes is adequate for completing multiple lane changes. +4. Intersection requirements are met (conditions are parameterized). +5. Crosswalk requirements are satisfied (conditions are parameterized). +6. Traffic light regulations are adhered to (conditions are parameterized). +7. The lane change can be completed after passing a parked vehicle. +8. The lane change is deemed safe to execute. The following flow chart illustrates the validity check. ```plantuml @startuml -skinparam monochrome true skinparam defaultTextAlignment center -skinparam noteTextAlignment left +skinparam backgroundColor #White -title Selecting Valid Candidate Paths start -:**INPUT** std::vector input_paths; +if (Check if start point is valid by check if it is covered by neighbour lanes polygon) then (not covered) + #LightPink:Reject path; + stop +else (covered) +endif -partition selectValidPaths { -:**INITIALIZE** std::vector valid_paths; +group check for distance #LightYellow + :Calculate total length and goal related distances; + if (total lane change length considering single lane change > distance from current pose to end of current lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif -:idx = 0; + if (goal is in current lanes) then (yes) + if (total lane change length considering multiple lane changes > distance from ego to goal along current lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + else (no) + endif -while (idx < input_paths.size()?) + if (target lanes is empty) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + if (total lane change length considering multiple lane changes > distance from ego to the end of target lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif +end group -:path = input_paths.at(idx); -partition hasEnoughDistance { -if(lane_change_total_distance < distance to end of current lanes -&& -lane_change_total_distance < distance to the next intersection -&& -lane_change_total_distance < distance from current pose to the goal -&& -lane_change_total_distance < distance to crosswalk -&& -goal is in route -) then (true) -:path_validity = true; -else (\n false) -:path_validity = false; +group evaluate on Crosswalk #LightCyan +if (regulate_on_crosswalk and not enough length to crosswalk) then (yes) + if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) + #LightPink:Reject path; + stop + else (no) + :Allow lane change in crosswalk; + endif +else (no) endif -} +end group -if(path_validity == true)then (true) - -:valid_paths.push_back(path); +group evaluate on Intersection #LightGreen +if (regulate_on_intersection and not enough length to intersection) then (yes) + if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) + #LightPink:Reject path; + stop + else (no) + :Allow lane change in intersection; + endif +else (no) +endif +end group + +group evaluate on Traffic Light #Lavender +if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes) + #LightPink:Reject path; + stop +elseif (stopped at red traffic light within distance) then (yes) + #LightPink:Reject path; + stop +else (no) +endif +end group -else (\nfalse) +if (ego is not stuck but parked vehicle exists in target lane) then (yes) + #LightPink:Reject path; + stop +else (no) endif -:++idx; -endwhile (false) -:**RETURN** valid_paths; +if (is safe to perform lane change) then (yes) + #Cyan:Return candidate path list; + stop +else (no) + #LightPink:Reject path; +endif -} stop + @enduml ``` @@ -165,6 +298,27 @@ First, we divide the target objects into obstacles in the target lane, obstacles Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. + +
+ + + + + +
+
+
Without Lane Expansion
+ Without lane expansion +
+ 		+
+
With Lane Expansion
+ With lane expansion +
+
+
+ ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. @@ -333,45 +487,38 @@ The last behavior will also occur if the ego vehicle has departed from the curre ### Essential lane change parameters -The following parameters are configurable in `lane_change.param.yaml`. - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------- | ------ | ------- | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | -| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | -| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | -| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | -| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | -| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | -| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | -| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | -| `target_object.car` | [-] | boolean | Include car objects for safety check | true | -| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | -| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | -| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | -| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | -| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | -| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | -| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +The following parameters are configurable in [lane_change.param.yaml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml) + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | +| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | +| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | +| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | ### Lane change regulations -| Name | Unit | Type | Description | Default value | -| :------------------------ | ---- | ------- | ------------------------------------- | ------------- | -| `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false | -| `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false | +| Name | Unit | Type | Description | Default value | +| :------------------------- | ---- | ------- | ---------------------------------------------------------- | ------------- | +| `regulation.crosswalk` | [-] | boolean | Allow lane change in between crosswalks | true | +| `regulation.intersection` | [-] | boolean | Allow lane change in between intersections | true | +| `regulation.traffic_light` | [-] | boolean | Allow lane change to be performed in between traffic light | true | ### Ego vehicle stuck detection @@ -380,9 +527,20 @@ The following parameters are configurable in `lane_change.param.yaml`. | `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | | `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | -### Collision checks during lane change +### Collision checks + +#### Target Objects -The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. +| Name | Unit | Type | Description | Default value | +| :------------------------- | ---- | ------- | ------------------------------------------- | ------------- | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | #### common @@ -391,53 +549,53 @@ The following parameters are configurable in `behavior_path_planner.param.yaml` | `safety_check.lane_expansion.left_offset` | [m] | double | Expand the left boundary of the detection area, allowing objects previously outside on the left to be detected and registered as targets. | 0.0 | | `safety_check.lane_expansion.right_offset` | [m] | double | Expand the right boundary of the detection area, allowing objects previously outside on the right to be detected and registered as targets. | 0.0 | -#### execution - -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | -| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | -| `safety_check.execution.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | -| `safety_check.execution.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `safety_check.execution.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | -| `safety_check.execution.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | -| `safety_check.execution.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | - -##### cancel - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.5 | -| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.5 | -| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.5 | -| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.5 | -| `safety_check.cancel.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | false | -| `safety_check.cancel.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.2 | -| `safety_check.cancel.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | false | -| `safety_check.cancel.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | false | -| `safety_check.cancel.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | - -##### stuck - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------------------------ | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | -| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | -| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | -| `safety_check.stuck.enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | -| `safety_check.stuck.prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `safety_check.stuck.check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | -| `safety_check.stuck.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | -| `safety_check.stuck.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +#### Additional parameters + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + +#### safety constraints during lane change path is computed + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | +| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + +##### safety constraints to cancel lane change path + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.0 | +| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.5 | +| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 0.8 | +| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | +| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | +| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | + +##### safety constraints used during lane change path is computed when ego is stuck + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | +| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | (\*1) the value must be negative. diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 74c6ad0893b23..1ab33514c5f24 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml @@ -89,7 +89,10 @@ stop_time: 3.0 # [s] # collision check - enable_prepare_segment_collision_check: true + enable_collision_check_for_prepare_phase: + general_lanes: false + intersection: true + turns: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] check_objects_on_current_lanes: true check_objects_on_other_lanes: true diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png index 411a72c30b88a..efd1df086544d 100644 Binary files a/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png and b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png differ diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png new file mode 100644 index 0000000000000..1793eb8f6e691 Binary files /dev/null and b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png differ diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png new file mode 100644 index 0000000000000..6b440d862c7ac Binary files /dev/null and b/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png differ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 3b8d2ad6e9931..7a76daff4bc55 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -172,8 +172,12 @@ class NormalLaneChange : public LaneChangeBase bool isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; + double get_max_velocity_for_safety_check() const; + bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool check_prepare_phase() const; + double calcMaximumLaneChangeLength( const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index e48f687a89035..fdaa15ff9bef9 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -112,7 +112,9 @@ struct LaneChangeParameters double max_longitudinal_acc{1.0}; // collision check - bool enable_prepare_segment_collision_check{true}; + bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; + bool enable_collision_check_for_prepare_phase_in_intersection{true}; + bool enable_collision_check_for_prepare_phase_in_turns{true}; double prepare_segment_ignore_object_velocity_thresh{0.1}; bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp index d8ba7cf19cfee..37d436f9949a2 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -21,6 +21,9 @@ #include +#include + +#include #include namespace behavior_path_planner::data::lane_change @@ -34,7 +37,17 @@ struct Debug CollisionCheckDebugMap collision_check_objects_after_approval; LaneChangeTargetObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; + geometry_msgs::msg::Pose ego_pose; + lanelet::ConstLanelets current_lanes; + lanelet::ConstLanelets target_lanes; + lanelet::ConstLanelets target_backward_lanes; double collision_check_object_debug_lifetime{0.0}; + double distance_to_end_of_current_lane{std::numeric_limits::max()}; + double distance_to_lane_change_finished{std::numeric_limits::max()}; + double distance_to_abort_finished{std::numeric_limits::max()}; + bool is_able_to_return_to_current_lane{false}; + bool is_stuck{false}; + bool is_abort{false}; void reset() { @@ -44,7 +57,18 @@ struct Debug filtered_objects.current_lane.clear(); filtered_objects.target_lane.clear(); filtered_objects.other_lane.clear(); + execution_area.points.clear(); + current_lanes.clear(); + target_lanes.clear(); + target_backward_lanes.clear(); + collision_check_object_debug_lifetime = 0.0; + distance_to_end_of_current_lane = std::numeric_limits::max(); + distance_to_lane_change_finished = std::numeric_limits::max(); + distance_to_abort_finished = std::numeric_limits::max(); + is_able_to_return_to_current_lane = false; + is_stuck = false; + is_abort = false; } }; } // namespace behavior_path_planner::data::lane_change diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 8863cabdba008..bc4413e1c69d3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -20,6 +20,7 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include +#include #include #include @@ -42,7 +43,9 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); -MarkerArray createDebugMarkerArray(const Debug & debug_data); +MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); +MarkerArray createDebugMarkerArray( + const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 35cc02e867557..76331bd98eb9c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -23,6 +23,7 @@ #include "rclcpp/logger.hpp" #include +#include #include #include @@ -31,6 +32,7 @@ #include +#include #include #include @@ -185,7 +187,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); @@ -220,6 +222,73 @@ lanelet::ConstLanelets generateExpandedLanelets( * @return rclcpp::Logger The logger instance configured for the specified lane change type. */ rclcpp::Logger getLogger(const std::string & type); + +/** + * @brief Computes the current footprint of the ego vehicle based on its pose and size. + * + * This function calculates the 2D polygon representing the current footprint of the ego vehicle. + * The footprint is determined by the vehicle's pose and its dimensions, including the distance + * from the base to the front and rear ends of the vehicle, as well as its width. + * + * @param ego_pose The current pose of the ego vehicle. + * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal + * offset, rear overhang, and width. + * + * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. + */ +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); + +/** + * @brief Checks if the given polygon is within an intersection area. + * + * This function evaluates whether a specified polygon is located within the bounds of an + * intersection. It identifies the intersection area by checking the attributes of the provided + * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function + * then checks if the polygon is fully contained within this area. + * + * @param route_handler a shared pointer to the route_handler + * @param lanelet A lanelet to check against the + * intersection area. + * @param polygon The polygon to check for containment within the intersection area. + * + * @return bool True if the polygon is within the intersection area, false otherwise. + */ +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon); + +/** + * @brief Determines if a polygon is within lanes designated for turning. + * + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). + * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's + * area. + * + * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. + * @param polygon The polygon to be checked for its presence within turn direction lanes. + * + * @return bool True if the polygon is within a lane designated for turning, false if it is within a + * straight lane or no turn direction is specified. + */ +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double time); } // namespace behavior_path_planner::utils::lane_change namespace behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 46b65a3ef4e79..523a5349aaef6 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -85,9 +85,9 @@ void LaneChangeInterface::updateData() if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } - updateDebugMarker(); module_type_->resetStopPose(); + updateDebugMarker(); } void LaneChangeInterface::postProcess() @@ -97,6 +97,7 @@ void LaneChangeInterface::postProcess() post_process_safety_status_ = module_type_->evaluateApprovedPathWithUnsafeHysteresis(safety_status); } + updateDebugMarker(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -196,6 +197,7 @@ bool LaneChangeInterface::canTransitSuccessState() auto log_debug_throttled = [&](std::string_view message) -> void { RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; + updateDebugMarker(); if (module_type_->specialExpiredCheck() && isWaitingApproval()) { log_debug_throttled("Run specialExpiredCheck."); @@ -225,6 +227,7 @@ bool LaneChangeInterface::canTransitFailureState() RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; + updateDebugMarker(); log_debug_throttled(__func__); if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { @@ -300,7 +303,7 @@ void LaneChangeInterface::updateDebugMarker() const return; } using marker_utils::lane_change_markers::createDebugMarkerArray; - debug_marker_ = createDebugMarkerArray(module_type_->getDebugData()); + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index 9f1c3c13bfadc..81d4b86afa094 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -68,8 +68,12 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); // collision check - p.enable_prepare_segment_collision_check = - getOrDeclareParameter(*node, parameter("enable_prepare_segment_collision_check")); + p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); + p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.intersection")); + p.enable_collision_check_for_prepare_phase_in_turns = + getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( *node, parameter("prepare_segment_ignore_object_velocity_thresh")); p.check_objects_on_current_lanes = diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 9524d47e522e5..db96f7c83d72a 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -567,15 +567,20 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); + const auto distance_to_lane_change_end = std::invoke([&]() { + auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } + if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { + distance_to_end = std::min( + distance_to_end, + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); + } + + return std::max(0.0, distance_to_end) - lane_change_buffer; + }); - return (std::max(0.0, distance_to_end) - lane_change_buffer) < threshold; + lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; + return distance_to_lane_change_end < threshold; } bool NormalLaneChange::hasFinishedLaneChange() const @@ -593,6 +598,10 @@ bool NormalLaneChange::hasFinishedLaneChange() const } const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; + + lane_change_debug_.distance_to_lane_change_finished = + dist_to_lane_change_end + finish_judge_buffer; + if (!reach_lane_change_end) { return false; } @@ -600,22 +609,23 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; - if (!reach_target_lane) { - return false; - } - return true; + lane_change_debug_.distance_to_lane_change_finished = arc_length.distance; + + return reach_target_lane; } bool NormalLaneChange::isAbleToReturnCurrentLane() const { if (status_.lane_change_path.path.points.size() < 2) { + lane_change_debug_.is_able_to_return_to_current_lane = false; return false; } if (!utils::isEgoWithinOriginalLane( status_.current_lanes, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { + lane_change_debug_.is_able_to_return_to_current_lane = false; return false; } @@ -633,12 +643,15 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; - return utils::isEgoWithinOriginalLane( + auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( status_.current_lanes, estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); + lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; + return is_ego_within_original_lane; } } + lane_change_debug_.is_able_to_return_to_current_lane = true; return true; } @@ -679,17 +692,18 @@ bool NormalLaneChange::isAbleToStopSafely() const bool NormalLaneChange::hasFinishedAbort() const { if (!abort_path_) { + lane_change_debug_.is_abort = true; return true; } const auto distance_to_finish = calcSignedArcLength( abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); + lane_change_debug_.distance_to_abort_finished = distance_to_finish; - if (distance_to_finish < 0.0) { - return true; - } + const auto has_finished_abort = distance_to_finish < 0.0; + lane_change_debug_.is_abort = has_finished_abort; - return false; + return has_finished_abort; } bool NormalLaneChange::isAbortState() const @@ -706,6 +720,7 @@ bool NormalLaneChange::isAbortState() const return false; } + lane_change_debug_.is_abort = true; return true; } int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const @@ -860,6 +875,10 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( route_handler, target_lanes, current_pose, backward_length); + lane_change_debug_.current_lanes = current_lanes; + lane_change_debug_.target_lanes = target_lanes; + lane_change_debug_.target_backward_lanes = target_backward_lanes; + // filter objects to get target object index const auto target_obj_index = filterObject(objects, current_lanes, target_lanes, target_backward_lanes); @@ -870,23 +889,27 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( target_objects.other_lane.reserve(target_obj_index.other_lane.size()); // objects in current lane + const auto is_check_prepare_phase = check_prepare_phase(); for (const auto & obj_idx : target_obj_index.current_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.current_lane.push_back(extended_object); } // objects in target lane for (const auto & obj_idx : target_obj_index.target_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.target_lane.push_back(extended_object); } // objects in other lane for (const auto & obj_idx : target_obj_index.other_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.other_lane.push_back(extended_object); } @@ -950,8 +973,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - const auto extended_object = - utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + const auto extended_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, check_prepare_phase()); const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); @@ -1270,9 +1293,8 @@ bool NormalLaneChange::getLaneChangePaths( (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration); - const double prepare_length = - current_velocity * prepare_duration + - 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); + const auto prepare_length = utils::lane_change::calcPhaseLength( + current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); @@ -1324,9 +1346,9 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::calcLaneChangingAcceleration( initial_lane_changing_velocity, max_path_velocity, lane_changing_time, sampled_longitudinal_acc); - const auto lane_changing_length = - initial_lane_changing_velocity * lane_changing_time + - 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; + const auto lane_changing_length = utils::lane_change::calcPhaseLength( + initial_lane_changing_velocity, getCommonParam().max_vel, + longitudinal_acc_on_lane_changing, lane_changing_time); const auto terminal_lane_changing_velocity = std::min( initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, getCommonParam().max_vel); @@ -1945,7 +1967,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - current_debug_data.second); + get_max_velocity_for_safety_check(), current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2040,9 +2062,21 @@ bool NormalLaneChange::isVehicleStuck( return false; } +double NormalLaneChange::get_max_velocity_for_safety_check() const +{ + const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; + if (external_velocity_limit_ptr) { + return std::min( + static_cast(external_velocity_limit_ptr->max_velocity), getCommonParam().max_vel); + } + + return getCommonParam().max_vel; +} + bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { if (current_lanes.empty()) { + lane_change_debug_.is_stuck = false; return false; // can not check } @@ -2062,7 +2096,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); - return isVehicleStuck(current_lanes, detection_distance); + auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + + lane_change_debug_.is_stuck = is_vehicle_stuck; + return is_vehicle_stuck; } void NormalLaneChange::setStopPose(const Pose & stop_pose) @@ -2090,4 +2127,41 @@ void NormalLaneChange::updateStopTime() stop_watch_.tic("stop_time"); } +bool NormalLaneChange::check_prepare_phase() const +{ + const auto & route_handler = getRouteHandler(); + const auto & vehicle_info = getCommonParam().vehicle_info; + + const auto check_in_general_lanes = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG( + logger_, "Unable to get current lane. Default to %s.", + (check_in_general_lanes ? "true" : "false")); + return check_in_general_lanes; + } + + const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + + const auto check_in_intersection = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { + return false; + } + + return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); + }); + + const auto check_in_turns = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { + return false; + } + + return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); + }); + + return check_in_intersection || check_in_turns || check_in_general_lanes; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 67594823eb112..9e44e51d8b72d 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -17,15 +17,18 @@ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include +#include #include #include #include +#include #include #include #include #include +#include #include #include @@ -122,8 +125,39 @@ MarkerArray showFilteredObjects( return marker_array; } -MarkerArray createDebugMarkerArray(const Debug & debug_data) +MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) { + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "execution_info", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::white()); + }; + + MarkerArray marker_array; + + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = ego_pose; + safety_check_info_text.pose.position.z += 4.0; + + std::ostringstream ss; + + ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5) + << debug_data.distance_to_end_of_current_lane + << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished + << (debug_data.is_stuck ? "\nVehicleStuck" : "") + << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "") + << (debug_data.is_abort ? "\nAborting" : "") + << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished; + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + return marker_array; +} + +MarkerArray createDebugMarkerArray( + const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) +{ + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; @@ -141,6 +175,20 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data) tier4_autoware_utils::appendMarkerArray(added, &debug_marker); }; + if (!debug_data.execution_area.points.empty()) { + add(createPolygonMarkerArray( + debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + } + + add(showExecutionInfo(debug_data, ego_pose)); + + // lanes + add(laneletsAsTriangleMarkerArray( + "current_lanes", debug_data.current_lanes, colors::light_yellow(0.2))); + add(laneletsAsTriangleMarkerArray("target_lanes", debug_data.target_lanes, colors::aqua(0.2))); + add(laneletsAsTriangleMarkerArray( + "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); + add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); add(showFilteredObjects( debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, @@ -162,11 +210,6 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data) "ego_and_target_polygon_relation_after_approval")); } - if (!debug_data.execution_area.points.empty()) { - add(createPolygonMarkerArray( - debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); - } - return debug_marker; } } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index bae733610905b..97fcc841a546e 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -31,7 +31,13 @@ #include #include #include +#include #include +#include + +#include + +#include #include #include @@ -435,7 +441,8 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); - if (s_end - s_start < lane_changing_length) { + constexpr double epsilon = 1e-4; + if (s_end - s_start + epsilon < lane_changing_length) { return PathWithLaneId(); } @@ -1086,7 +1093,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) { ExtendedPredictedObject extended_object; extended_object.uuid = object.object_id; @@ -1096,8 +1103,6 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & check_at_prepare_phase = - lane_change_parameters.enable_prepare_segment_collision_check; const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; @@ -1155,6 +1160,53 @@ rclcpp::Logger getLogger(const std::string & type) { return rclcpp::get_logger("lane_change").get_child(type); } + +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info) +{ + const auto base_to_front = ego_info.max_longitudinal_offset_m; + const auto base_to_rear = ego_info.rear_overhang_m; + const auto width = ego_info.vehicle_width_m; + + return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); +} + +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon) +{ + const std::string id = lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto lanelet_polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); +} + +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +{ + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "else" || turn_direction == "straight") { + return false; + } + + return !boost::geometry::disjoint( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); +} + +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} } // namespace behavior_path_planner::utils::lane_change namespace behavior_path_planner::utils::lane_change::debug @@ -1194,4 +1246,5 @@ geometry_msgs::msg::Polygon createExecutionArea( return polygon; } + } // namespace behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 99e312f98f100..5a49f4d9ab66e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -103,6 +104,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; + rclcpp::Subscription::SharedPtr + external_limit_max_velocity_subscriber_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -142,6 +145,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); void onLateralOffset(const LateralOffset::ConstSharedPtr msg); + void on_external_velocity_limiter( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + SetParametersResult onSetParam(const std::vector & parameters); OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e41061a2ffbd2..d250fd97c3aec 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -112,6 +112,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod current_scenario_ = std::make_shared(*msg); }, createSubscriptionOptions(this)); + external_limit_max_velocity_subscriber_ = + create_subscription( + "/planning/scenario_planning/max_velocity", 1, + std::bind(&BehaviorPathPlannerNode::on_external_velocity_limiter, this, _1), + createSubscriptionOptions(this)); // route_handler vector_map_subscriber_ = create_subscription( @@ -312,6 +317,10 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("operation_mode"); } + if (!planner_data_->occupancy_grid) { + return missing("occupancy_grid"); + } + return true; } @@ -817,6 +826,19 @@ void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSha const std::lock_guard lock(mutex_pd_); planner_data_->operation_mode = msg; } + +void BehaviorPathPlannerNode::on_external_velocity_limiter( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + // Note: Using this parameter during path planning might cause unexpected deceleration or jerk. + // Therefore, do not use it for anything other than safety checks. + if (!msg) { + return; + } + + const std::lock_guard lock(mutex_pd_); + planner_data_->external_limit_max_velocity = msg; +} void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index d196f2bc551bb..09f3c2c66bcd4 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; +using tier4_planning_msgs::msg::VelocityLimit; using unique_identifier_msgs::msg::UUID; struct TrafficSignalStamped @@ -161,6 +163,7 @@ struct PlannerData std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + VelocityLimit::ConstSharedPtr external_limit_max_velocity{}; mutable std::vector drivable_area_expansion_prev_path_poses{}; mutable std::vector drivable_area_expansion_prev_curvatures{}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 13009d5114439..53c4706e49c10 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -141,7 +141,7 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug); + const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug); bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e15a3c164bef1..6390c45376b37 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,8 @@ #include #include +#include + namespace behavior_path_planner::utils::path_safety_checker { @@ -560,7 +562,7 @@ bool checkCollision( { const auto collided_polygons = getCollidedPolygons( planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, - rss_parameters, hysteresis_factor, debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug); return collided_polygons.empty(); } @@ -570,7 +572,7 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - double hysteresis_factor, CollisionCheckDebug & debug) + double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug) { { debug.ego_predicted_path = predicted_ego_path; @@ -586,7 +588,7 @@ std::vector getCollidedPolygons( // get object information at current time const auto & obj_pose = obj_pose_with_poly.pose; const auto & obj_polygon = obj_pose_with_poly.poly; - const auto & object_velocity = obj_pose_with_poly.velocity; + const auto object_velocity = obj_pose_with_poly.velocity; // get ego information at current time // Note: we can create these polygons in advance. However, it can decrease the readability and @@ -599,7 +601,7 @@ std::vector getCollidedPolygons( } const auto & ego_pose = interpolated_data->pose; const auto & ego_polygon = interpolated_data->poly; - const auto & ego_velocity = interpolated_data->velocity; + const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit); // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index b17db7f907471..46469de68149e 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -27,6 +27,7 @@ shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 + maximum_longitudinal_deviation: 1.0 # Note, should be the same or less than planning validator's "longitudinal_distance_deviation" lateral_jerk: 0.5 lateral_acceleration_sampling_num: 3 minimum_lateral_acc: 0.15 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 1e32237ffd870..0e85c1d0e76b3 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -84,6 +84,7 @@ struct StartPlannerParameters double minimum_lateral_acc{0.0}; double maximum_curvature{0.0}; // maximum curvature considered in the path generation double deceleration_interval{0.0}; + double maximum_longitudinal_deviation{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; double geometric_collision_check_distance_from_end; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 050d591128a15..794577d7cc7aa 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -88,6 +88,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); + p.maximum_longitudinal_deviation = + node->declare_parameter(ns + "maximum_longitudinal_deviation"); // geometric pull out p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); p.geometric_collision_check_distance_from_end = @@ -424,6 +426,8 @@ void StartPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam( + parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index c6a56139e63d2..592f57ed97dcf 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -111,6 +111,26 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); if (cropped_path.points.empty()) continue; + + // check that the path is not cropped in excess and there is not excessive longitudinal + // deviation between the first 2 points + auto validate_cropped_path = [&](const auto & cropped_path) -> bool { + if (cropped_path.points.size() < 2) return false; + const double max_long_offset = parameters_.maximum_longitudinal_deviation; + const size_t start_segment_idx_after_crop = + motion_utils::findFirstNearestIndexWithSoftConstraints(cropped_path.points, start_pose); + + // if the start segment id after crop is not 0, then the cropping is not excessive + if (start_segment_idx_after_crop != 0) return true; + + const auto long_offset_to_closest_point = motion_utils::calcLongitudinalOffsetToSegment( + cropped_path.points, start_segment_idx_after_crop, start_pose.position); + const auto long_offset_to_next_point = motion_utils::calcLongitudinalOffsetToSegment( + cropped_path.points, start_segment_idx_after_crop + 1, start_pose.position); + return std::abs(long_offset_to_closest_point - long_offset_to_next_point) < max_long_offset; + }; + + if (!validate_cropped_path(cropped_path)) continue; shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index 77f42822a6b95..7c79d1c288a0d 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -108,6 +108,14 @@ You can choose whether to use this feature by parameter of `use_partition_lanele ![brief](./docs/exclude_obstacles_by_partition.svg) +##### Exclude obstacles by label + +This module only acts on target obstacles defined by the `target_obstacle_types` parameter. If an obstacle of a type not specified by said parameter is detected, it will be ignored by this module. + +##### Exclude obstacles that are already on the ego vehicle's path + +In the cases were an obstacle is already on the ego vehicle's path, it cannot "cut in" into the ego's path anymore (which is the situation this module tries to handle) so it might be useful to exclude obstacles already on the vehicle's footprint path. By setting the parameter `exclude_obstacles_already_in_path` to true, this module will exclude the obstacles that are considered to be already on the ego vehicle's path for more than `keep_obstacle_on_path_time_threshold`. The module considers the ego vehicle's closest path point to each obstacle's current position, and determines the lateral distance between the obstacle and the right and left sides of the ego vehicle. If the obstacle is located within the left and right extremes of the vehicle for that pose, then it is considered to be inside the ego vehicle's footprint path and it is excluded. The virtual width of the vehicle used to detect if an obstacle is within the path or not, can be adjusted by the `ego_footprint_extra_margin` parameter. + ##### Exclude obstacles that cross the ego vehicle's "cut line" This module can exclude obstacles that have predicted paths that will cross the back side of the ego vehicle. It excludes obstacles if their predicted path crosses the ego's "cut line". The "cut line" is a virtual line segment that is perpendicular to the ego vehicle and that passes through the ego's base link. diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index 6e28b230757ba..6d0d9571bf43b 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,6 +3,7 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points target_obstacle_types: ["PEDESTRIAN", "BICYCLE", "MOTORCYCLE"] # [-] Obstacle types considered by this module + exclude_obstacles_already_in_path: true # If the obstacle is already on the ego's path footprint, ignore it. use_partition_lanelet: true # [-] whether to use partition lanelet map data suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet use_ego_cut_line: true # [-] filter obstacles that pass the backside of ego: if a dynamic obstacle's predicted path intersects this line, it is ignored @@ -14,6 +15,9 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision ego_cut_line_length: 3.0 # The width of the ego's cut line + ego_footprint_extra_margin: 0.5 # [m] expand the ego vehicles' footprint by this value on all sides when building the ego footprint path + keep_obstacle_on_path_time_threshold: 1.0 # [s] How much time a previous run out target obstacle is kept in the run out candidate list if it enters the ego path. + keep_stop_point_time: 1.0 # [s] If a stop point is issued by this module, keep the stop point for this many seconds. Only works if approach is disabled # Parameter to create abstracted dynamic obstacles dynamic_obstacle: diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 0af59417a23b2..9b94c8b681942 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -373,6 +374,7 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta } dynamic_obstacle.classifications = predicted_object.classification; dynamic_obstacle.shape = predicted_object.shape; + dynamic_obstacle.uuid = predicted_object.object_id; // get predicted paths of predicted_objects for (const auto & path : predicted_object.kinematics.predicted_paths) { @@ -418,6 +420,7 @@ std::vector DynamicObstacleCreatorForObjectWithoutPath::createD param_.max_prediction_time); predicted_path.confidence = 1.0; dynamic_obstacle.predicted_paths.emplace_back(predicted_path); + dynamic_obstacle.uuid = predicted_object.object_id; dynamic_obstacles.emplace_back(dynamic_obstacle); } @@ -490,7 +493,7 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta param_.max_prediction_time); predicted_path.confidence = 1.0; dynamic_obstacle.predicted_paths.emplace_back(predicted_path); - + dynamic_obstacle.uuid = tier4_autoware_utils::generateUUID(); dynamic_obstacles.emplace_back(dynamic_obstacle); } diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 5217193c9673e..f020da2abd045 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 9ee467feec758..1c85a22f57bf6 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -62,6 +62,8 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) getOrDeclareParameter>(node, ns + ".target_obstacle_types"); p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); p.use_ego_cut_line = getOrDeclareParameter(node, ns + ".use_ego_cut_line"); + p.exclude_obstacles_already_in_path = + getOrDeclareParameter(node, ns + ".exclude_obstacles_already_in_path"); p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk"); p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -71,6 +73,11 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.detection_span = getOrDeclareParameter(node, ns + ".detection_span"); p.min_vel_ego_kmph = getOrDeclareParameter(node, ns + ".min_vel_ego_kmph"); p.ego_cut_line_length = getOrDeclareParameter(node, ns + ".ego_cut_line_length"); + p.ego_footprint_extra_margin = + getOrDeclareParameter(node, ns + ".ego_footprint_extra_margin"); + p.keep_obstacle_on_path_time_threshold = + getOrDeclareParameter(node, ns + ".keep_obstacle_on_path_time_threshold"); + p.keep_stop_point_time = getOrDeclareParameter(node, ns + ".keep_stop_point_time"); } { diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index cd15a3fb0629f..da826e1cf0cf6 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,7 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using object_recognition_utils::convertLabelToString; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, @@ -103,16 +105,18 @@ bool RunOutModule::modifyPathVelocity( debug_ptr_->setDebugValues(DebugValues::TYPE::NUM_OBSTACLES, dynamic_obstacles.size()); const auto filtered_obstacles = std::invoke([&]() { + // Only target_obstacle_types are considered. + const auto target_obstacles = excludeObstaclesBasedOnLabel(dynamic_obstacles); // extract obstacles using lanelet information const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); - - if (!planner_param_.run_out.use_ego_cut_line) return partition_excluded_obstacles; - + excludeObstaclesOutSideOfPartition(target_obstacles, extended_smoothed_path, current_pose); // extract obstacles that cross the ego's cut line const auto ego_cut_line_excluded_obstacles = excludeObstaclesCrossingEgoCutLine(partition_excluded_obstacles, current_pose); - return ego_cut_line_excluded_obstacles; + // extract obstacles already on the ego's path + const auto obstacles_outside_ego_path = + excludeObstaclesOnEgoPath(ego_cut_line_excluded_obstacles, extended_smoothed_path); + return obstacles_outside_ego_path; }); // record time for obstacle creation @@ -140,6 +144,16 @@ bool RunOutModule::modifyPathVelocity( debug_ptr_->setDebugValues( DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); + // If no new obstacle is detected, we check if the stop point should be maintained for more time. + auto should_maintain_stop_point = [&](const bool is_stopping_point_inserted) -> bool { + if (!stop_point_generation_time_.has_value() || is_stopping_point_inserted) { + return false; + } + const auto now = clock_->now().seconds(); + const double elapsed_time_since_stop_wall_was_generated = + (now - stop_point_generation_time_.value()); + return elapsed_time_since_stop_wall_was_generated < planner_param_.run_out.keep_stop_point_time; + }; // insert stop point for the detected obstacle if (planner_param_.approaching.enable) { // after a certain amount of time has elapsed since the ego stopped, @@ -148,7 +162,23 @@ bool RunOutModule::modifyPathVelocity( dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path); } else { // just insert zero velocity for stopping - insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); + std::optional stop_point; + const bool is_stopping_point_inserted = insertStoppingVelocity( + dynamic_obstacle, current_pose, current_vel, current_acc, *path, stop_point); + + stop_point_generation_time_ = (is_stopping_point_inserted) + ? std::make_optional(clock_->now().seconds()) + : stop_point_generation_time_; + last_stop_point_ = (is_stopping_point_inserted) ? stop_point : last_stop_point_; + + const bool is_maintain_stop_point = should_maintain_stop_point(is_stopping_point_inserted); + if (is_maintain_stop_point) { + insertStopPoint(last_stop_point_, *path); + // debug + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); + debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + *last_stop_point_, planner_param_.vehicle_param.base_to_front, 0, 0)); + } } // apply max jerk limit if the ego can't stop with specified max jerk and acc @@ -226,10 +256,8 @@ std::optional RunOutModule::detectCollision( debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points); debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point); - return obstacle_selected; } - // no collision detected first_detected_time_.reset(); return {}; @@ -327,27 +355,79 @@ std::vector RunOutModule::createVehiclePolygon( return vehicle_poly; } +std::vector RunOutModule::excludeObstaclesOnEgoPath( + const std::vector & dynamic_obstacles, const PathWithLaneId & path) +{ + using motion_utils::findNearestIndex; + using tier4_autoware_utils::calcOffsetPose; + if (!planner_param_.run_out.exclude_obstacles_already_in_path || path.points.empty()) { + return dynamic_obstacles; + } + const auto footprint_extra_margin = planner_param_.run_out.ego_footprint_extra_margin; + const auto vehicle_width = planner_param_.vehicle_param.width; + const auto vehicle_with_with_margin_halved = (vehicle_width + footprint_extra_margin) / 2.0; + const double time_threshold_for_prev_collision_obstacle = + planner_param_.run_out.keep_obstacle_on_path_time_threshold; + + std::vector obstacles_outside_of_path; + std::for_each(dynamic_obstacles.begin(), dynamic_obstacles.end(), [&](const auto & o) { + const auto idx = findNearestIndex(path.points, o.pose); + if (!idx) { + obstacles_outside_of_path.push_back(o); + return; + } + const auto object_position = o.pose.position; + const auto closest_ego_pose = path.points.at(*idx).point.pose; + const auto vehicle_left_pose = + tier4_autoware_utils::calcOffsetPose(closest_ego_pose, 0, vehicle_with_with_margin_halved, 0); + const auto vehicle_right_pose = tier4_autoware_utils::calcOffsetPose( + closest_ego_pose, 0, -vehicle_with_with_margin_halved, 0); + const double signed_distance_from_left = + tier4_autoware_utils::calcLateralDeviation(vehicle_left_pose, object_position); + const double signed_distance_from_right = + tier4_autoware_utils::calcLateralDeviation(vehicle_right_pose, object_position); + + // If object is outside of the ego path, include it in list of possible target objects + // It is also deleted from the path of objects inside ego path + const auto obstacle_uuid_hex = tier4_autoware_utils::toHexString(o.uuid); + if (signed_distance_from_left > 0.0 || signed_distance_from_right < 0.0) { + obstacles_outside_of_path.push_back(o); + obstacle_in_ego_path_times_.erase(obstacle_uuid_hex); + return; + } + + const auto it = obstacle_in_ego_path_times_.find(obstacle_uuid_hex); + // This obstacle is first detected inside the ego path, we keep it. + if (it == obstacle_in_ego_path_times_.end()) { + const auto now = clock_->now().seconds(); + obstacle_in_ego_path_times_.insert(std::make_pair(obstacle_uuid_hex, now)); + obstacles_outside_of_path.push_back(o); + return; + } + + // if the obstacle has been on the ego path for more than a threshold time, it is excluded + const auto first_detection_inside_path_time = it->second; + const auto now = clock_->now().seconds(); + const double elapsed_time_since_detection_inside_of_path = + (now - first_detection_inside_path_time); + if (elapsed_time_since_detection_inside_of_path < time_threshold_for_prev_collision_obstacle) { + obstacles_outside_of_path.push_back(o); + return; + } + }); + return obstacles_outside_of_path; +} + std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, std::vector poly, const float travel_time, const std::vector> & crosswalk_lanelets) const { - using object_recognition_utils::convertLabelToString; const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); // check collision for each objects std::vector obstacles_collision; for (const auto & obstacle : dynamic_obstacles) { - // get classification that has highest probability - const auto classification = - convertLabelToString(run_out_utils::getHighestProbLabel(obstacle.classifications)); - const auto & target_obstacle_types = planner_param_.run_out.target_obstacle_types; - // detect only pedestrians, bicycles or motorcycles - const auto it = - std::find(target_obstacle_types.begin(), target_obstacle_types.end(), classification); - if (it == target_obstacle_types.end()) { - continue; - } // calculate predicted obstacle pose for min velocity and max velocity const auto predicted_obstacle_pose_min_vel = calcPredictedObstaclePose(obstacle.predicted_paths, travel_time, obstacle.min_velocity_mps); @@ -661,14 +741,14 @@ std::optional RunOutModule::calcStopPoint( return stop_point; } -void RunOutModule::insertStopPoint( +bool RunOutModule::insertStopPoint( const std::optional stop_point, autoware_auto_planning_msgs::msg::PathWithLaneId & path) { // no stop point if (!stop_point) { RCLCPP_DEBUG_STREAM(logger_, "already has same point"); - return; + return false; } // find nearest point index behind the stop point @@ -680,15 +760,15 @@ void RunOutModule::insertStopPoint( if ( insert_idx == path.points.size() - 1 && planning_utils::isAheadOf(*stop_point, path.points.at(insert_idx).point.pose)) { - return; + return false; } // to PathPointWithLaneId autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; - planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); + return true; } void RunOutModule::insertVelocityForState( @@ -750,14 +830,24 @@ void RunOutModule::insertVelocityForState( } } -void RunOutModule::insertStoppingVelocity( +bool RunOutModule::insertStoppingVelocity( const std::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, PathWithLaneId & output_path) { - const auto stop_point = + std::optional stopping_point; + return insertStoppingVelocity( + dynamic_obstacle, current_pose, current_vel, current_acc, output_path, stopping_point); +} + +bool RunOutModule::insertStoppingVelocity( + const std::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + PathWithLaneId & output_path, std::optional & stopping_point) +{ + stopping_point = calcStopPoint(dynamic_obstacle, output_path, current_pose, current_vel, current_acc); - insertStopPoint(stop_point, output_path); + return insertStopPoint(stopping_point, output_path); } void RunOutModule::insertApproachingVelocity( @@ -823,6 +913,7 @@ std::vector RunOutModule::excludeObstaclesCrossingEgoCutLine( const std::vector & dynamic_obstacles, const geometry_msgs::msg::Pose & current_pose) const { + if (!planner_param_.run_out.use_ego_cut_line) return dynamic_obstacles; std::vector extracted_obstacles; std::vector ego_cut_line; const double ego_cut_line_half_width = planner_param_.run_out.ego_cut_line_length / 2.0; @@ -837,6 +928,25 @@ std::vector RunOutModule::excludeObstaclesCrossingEgoCutLine( return extracted_obstacles; } +std::vector RunOutModule::excludeObstaclesBasedOnLabel( + const std::vector & dynamic_obstacles) const +{ + std::vector output; + const auto & target_obstacle_types = planner_param_.run_out.target_obstacle_types; + std::for_each(dynamic_obstacles.begin(), dynamic_obstacles.end(), [&](const auto obstacle) { + // get classification that has highest probability + const auto classification = + convertLabelToString(run_out_utils::getHighestProbLabel(obstacle.classifications)); + // include only pedestrians, bicycles or motorcycles + const auto it = + std::find(target_obstacle_types.begin(), target_obstacle_types.end(), classification); + if (it != target_obstacle_types.end()) { + output.push_back(obstacle); + } + }); + return output; +} + std::vector RunOutModule::excludeObstaclesOutSideOfPartition( const std::vector & dynamic_obstacles, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose) const @@ -909,6 +1019,7 @@ bool RunOutModule::isMomentaryDetection() return false; } + // No detection until now if (!first_detected_time_) { first_detected_time_ = std::make_shared(clock_->now()); return true; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 090f76ef30328..3db6051ab36e7 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include @@ -64,6 +66,11 @@ class RunOutModule : public SceneModuleInterface std::shared_ptr debug_ptr_; std::unique_ptr state_machine_; std::shared_ptr first_detected_time_; + std::optional stop_point_generation_time_; + std::optional last_stop_point_; + + std::optional last_stop_obstacle_uuid_; + std::unordered_map obstacle_in_ego_path_times_; // Function Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; @@ -115,7 +122,7 @@ class RunOutModule : public SceneModuleInterface const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc) const; - void insertStopPoint( + bool insertStopPoint( const std::optional stop_point, autoware_auto_planning_msgs::msg::PathWithLaneId & path); @@ -124,11 +131,16 @@ class RunOutModule : public SceneModuleInterface const PlannerParam & planner_param, const PathWithLaneId & smoothed_path, PathWithLaneId & output_path); - void insertStoppingVelocity( + bool insertStoppingVelocity( const std::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, PathWithLaneId & output_path); + bool insertStoppingVelocity( + const std::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + PathWithLaneId & output_path, std::optional & stopping_point); + void insertApproachingVelocity( const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float approaching_vel, const float approach_margin, PathWithLaneId & output_path); @@ -148,10 +160,16 @@ class RunOutModule : public SceneModuleInterface const std::vector & dynamic_obstacles, const geometry_msgs::msg::Pose & current_pose) const; + std::vector excludeObstaclesBasedOnLabel( + const std::vector & dynamic_obstacles) const; + std::vector excludeObstaclesOutSideOfPartition( const std::vector & dynamic_obstacles, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose) const; + std::vector excludeObstaclesOnEgoPath( + const std::vector & dynamic_obstacles, const PathWithLaneId & path); + void publishDebugValue( const PathWithLaneId & path, const std::vector extracted_obstacles, const std::optional & dynamic_obstacle, diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 54aa3395a0050..51b460482458f 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -21,13 +21,14 @@ #include #include +#include #include #include #include +#include #include #include - namespace behavior_velocity_planner { namespace run_out_utils @@ -60,10 +61,14 @@ struct RunOutParam bool suppress_on_crosswalk; bool specify_decel_jerk; bool use_ego_cut_line; + bool exclude_obstacles_already_in_path; double stop_margin; double passing_margin; double deceleration_jerk; double ego_cut_line_length; + double ego_footprint_extra_margin; + double keep_obstacle_on_path_time_threshold; + double keep_stop_point_time; float detection_distance; float detection_span; float min_vel_ego_kmph; @@ -187,6 +192,7 @@ struct DynamicObstacle std::vector classifications; Shape shape; std::vector predicted_paths; + unique_identifier_msgs::msg::UUID uuid; }; struct DynamicObstacleData diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 1ab5e9d5df487..3cc3801575116 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -292,6 +292,17 @@ void AstarSearch::setPath(const AstarNode & goal_node) // From the goal node to the start node const AstarNode * node = &goal_node; + // push exact goal pose first + geometry_msgs::msg::PoseStamped pose; + pose.header = header; + pose.pose = local2global(costmap_, goal_pose_); + + PlannerWaypoint pw; + pw.pose = pose; + pw.is_back = node->is_back; + waypoints_.waypoints.push_back(pw); + + // push astar nodes poses while (node != nullptr) { geometry_msgs::msg::PoseStamped pose; pose.header = header; diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 3b649168884e6..a9eb488c71377 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -7,8 +7,13 @@ The route is made of a sequence of lanes on a static map. Dynamic objects (e.g. pedestrians and other vehicles) and dynamic map information (e.g. road construction which blocks some lanes) are not considered during route planning. Therefore, the output topic is only published when the goal pose or check points are given and will be latched until the new goal pose or check points are given. -The core implementation does not depend on a map format. -In current Autoware.universe, only Lanelet2 map format is supported. +The core implementation does not depend on a map format. Any planning algorithms can be added as plugin modules. +In current Autoware.universe, only the plugin for Lanelet2 map format is supported. + +This package also manages routes for MRM. The `route_selector` node duplicates the `mission_planner` interface and provides it for normal and MRM respectively. +It distributes route requests and planning results according to current MRM operation state. + +![architecture](./media/architecture.drawio.svg) ## Interfaces @@ -28,31 +33,37 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Services -| Name | Type | Description | -| ------------------------------------------------ | ----------------------------------------- | ------------------------------------------------------------------------------------------- | -| `/planning/mission_planning/clear_route` | autoware_adapi_v1_msgs/srv/ClearRoute | route clear request | -| `/planning/mission_planning/set_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints. Assumed the vehicle is stopped. | -| `/planning/mission_planning/set_route` | autoware_adapi_v1_msgs/srv/SetRoute | route request with lanelet waypoints. Assumed the vehicle is stopped. | -| `/planning/mission_planning/change_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route change request with pose waypoints. This can be called when the vehicle is moving. | -| `/planning/mission_planning/change_route` | autoware_adapi_v1_msgs/srv/SetRoute | route change request with lanelet waypoints. This can be called when the vehicle is moving. | -| `~/srv/set_mrm_route` | autoware_adapi_v1_msgs/srv/SetRoutePoints | set emergency route. This can be called when the vehicle is moving. | -| `~/srv/clear_mrm_route` | std_srvs/srv/Trigger | clear emergency route. | +| Name | Type | Description | +| ------------------------------------------------------------------- | ---------------------------------------- | ------------------------------------------ | +| `/planning/mission_planning/mission_planner/clear_route` | tier4_planning_msgs/srv/ClearRoute | route clear request | +| `/planning/mission_planning/mission_planner/set_waypoint_route` | tier4_planning_msgs/srv/SetWaypointRoute | route request with lanelet waypoints. | +| `/planning/mission_planning/mission_planner/set_lanelet_route` | tier4_planning_msgs/srv/SetLaneletRoute | route request with pose waypoints. | +| `/planning/mission_planning/route_selector/main/clear_route` | tier4_planning_msgs/srv/ClearRoute | main route clear request | +| `/planning/mission_planning/route_selector/main/set_waypoint_route` | tier4_planning_msgs/srv/SetWaypointRoute | main route request with lanelet waypoints. | +| `/planning/mission_planning/route_selector/main/set_lanelet_route` | tier4_planning_msgs/srv/SetLaneletRoute | main route request with pose waypoints. | +| `/planning/mission_planning/route_selector/mrm/clear_route` | tier4_planning_msgs/srv/ClearRoute | mrm route clear request | +| `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | tier4_planning_msgs/srv/SetWaypointRoute | mrm route request with lanelet waypoints. | +| `/planning/mission_planning/route_selector/mrm/set_lanelet_route` | tier4_planning_msgs/srv/SetLaneletRoute | mrm route request with pose waypoints. | ### Subscriptions -| Name | Type | Description | -| --------------------- | ------------------------------------ | ---------------------- | -| `input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | -| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| ----------------------- | ------------------------------------ | ---------------------- | +| `~/input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | +| `~/input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | ### Publications -| Name | Type | Description | -| ---------------------------------------- | ------------------------------------- | ------------------------ | -| `/planning/mission_planning/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state | -| `/planning/mission_planning/route` | autoware_planning_msgs/LaneletRoute | route | -| `debug/route_marker` | visualization_msgs/msg/MarkerArray | route marker for debug | -| `debug/goal_footprint` | visualization_msgs/msg/MarkerArray | goal footprint for debug | +| Name | Type | Description | +| ------------------------------------------------------ | ----------------------------------- | ------------------------ | +| `/planning/mission_planning/state` | tier4_planning_msgs/msg/RouteState | route state | +| `/planning/mission_planning/route` | autoware_planning_msgs/LaneletRoute | route | +| `/planning/mission_planning/route_selector/main/state` | tier4_planning_msgs/msg/RouteState | main route state | +| `/planning/mission_planning/route_selector/main/route` | autoware_planning_msgs/LaneletRoute | main route | +| `/planning/mission_planning/route_selector/mrm/state` | tier4_planning_msgs/msg/RouteState | mrm route state | +| `/planning/mission_planning/route_selector/mrm/route` | autoware_planning_msgs/LaneletRoute | mrm route | +| `~/debug/route_marker` | visualization_msgs/msg/MarkerArray | route marker for debug | +| `~/debug/goal_footprint` | visualization_msgs/msg/MarkerArray | goal footprint for debug | ## Route section @@ -168,26 +179,16 @@ And there are three use cases that require reroute. - Emergency route - Goal modification -![rerouting_interface](./media/rerouting_interface.svg) - #### Route change API -- `change_route_points` -- `change_route` - -This is route change that the application makes using the API. It is used when changing the destination while driving or when driving a divided loop route. When the vehicle is driving on a MRM route, normal rerouting by this interface is not allowed. +It is used when changing the destination while driving or when driving a divided loop route. When the vehicle is driving on a MRM route, normal rerouting by this interface is not allowed. #### Emergency route -- `set_mrm_route` -- `clear_mrm_route` - -This interface for the MRM that pulls over the road shoulder. It has to be stopped as soon as possible, so a reroute is required. The MRM route has priority over the normal route. And if MRM route is cleared, try to return to the normal route also with a rerouting safety check. +The interface for the MRM that pulls over the road shoulder. It has to be stopped as soon as possible, so a reroute is required. The MRM route has priority over the normal route. And if MRM route is cleared, try to return to the normal route also with a rerouting safety check. ##### Goal modification -- `modified_goal` - This is a goal change to pull over, avoid parked vehicles, and so on by a planning component. If the modified goal is outside the calculated route, a reroute is required. This goal modification is executed by checking the local environment and path safety as the vehicle actually approaches the destination. And this modification is allowed for both normal_route and mrm_route. The new route generated here is sent to the AD API so that it can also be referenced by the application. Note, however, that the specifications here are subject to change in the future. diff --git a/planning/mission_planner/media/architecture.drawio.svg b/planning/mission_planner/media/architecture.drawio.svg new file mode 100644 index 0000000000000..36be0d683bc06 --- /dev/null +++ b/planning/mission_planner/media/architecture.drawio.svg @@ -0,0 +1,725 @@ + + + + + + + + + + + + + + + +
+
+
+ mission planner +
+ main module +
+
+
+ + mission planner... + + + + + + +
+
+
+ state +
+
+
+ + state + + + + + + +
+
+
+ route +
+
+
+ + route + + + + + + +
+
+
+ set waypoint +
+
+
+ + set waypoint + + + + + + +
+
+
+ clear +
+
+
+ + clear + + + + + + +
+
+
+ set lanelet +
+
+
+ + set lanelet + + + + + + +
+
+
+ main route interface +
+
+
+ + main route interface + + + + + + + + + + + +
+
+
+ state +
+
+
+ + state + + + + + + +
+
+
+ route +
+
+
+ + route + + + + + + +
+
+
+ set waypoint +
+
+
+ + set waypoint + + + + + + +
+
+
+ clear +
+
+
+ + clear + + + + + + +
+
+
+ set lanelet +
+
+
+ + set lanelet + + + + + + +
+
+
+ MRM route interface +
+
+
+ + MRM route interface + + + + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + + + + +
+
+
+ fail safe +
+
+
+ + fail safe + + + + + + + + + + +
+
+
+ route id +
+
+
+ + route id + + + + + + + + +
+
+
+ route +
+
+
+ + route + + + + + + + + +
+
+
+ state +
+
+
+ + state + + + + + + +
+
+
+ request +
+
+
+ + request + + + + + + +
+
+
+ route id +
+
+
+ + route id + + + + + + + + +
+
+
+ route +
+
+
+ + route + + + + + + + + +
+
+
+ state +
+
+
+ + state + + + + + + + + +
+
+
+ in mrm +
+
+
+ + in mrm + + + + + + +
+
+
+ main route data +
+
+
+ + main route data + + + + + + +
+
+
+ MRM route data +
+
+
+ + MRM route data + + + + + + + + + + + + +
+
+
+ route +
+
+
+ + route + + + + + + + + +
+
+
+ state +
+
+
+ + state + + + + + + +
+
+
+ set waypoint +
+
+
+ + set waypoint + + + + + + +
+
+
+ clear +
+
+
+ + clear + + + + + + +
+
+
+ set lanelet +
+
+
+ + set lanelet + + + + + + +
+
+
+ route interface +
+
+
+ + route interface + + + + + + +
+
+
+ route selector +
+
+
+ + route selector + + + + + + + + +
+
+
+ behavior planner +
+
+
+ + behavior planner + + + + + + + + +
+
+
+ modify goal +
+
+
+ + modify goal + + + + + + + + + +
+
+
+ mission planner +
+ plugin module +
+
+
+ + mission planner... + + + + + + +
+
+
+ mission planner +
+
+
+ + mission planner + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/mission_planner/media/rerouting_interface.svg b/planning/mission_planner/media/rerouting_interface.svg deleted file mode 100644 index e19c362611d30..0000000000000 --- a/planning/mission_planner/media/rerouting_interface.svg +++ /dev/null @@ -1,435 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- AD -
- API -
-
-
- - AD... - - - - - - - - - - - - -
-
-
- Mission Planner -
-
-
- - Mission Planner - - - - - - - - -
-
-
- set_route_points -
- (SetRoutePoints.srv) -
-
-
- - set_route_points... - - - - - - - - -
-
-
- set_route -
- (SetRoute.srv) -
-
-
- - set_route... - - - - - - - - -
-
-
- change_route_points -
- (SetRoutePoints.srv) -
-
-
- - change_route_points... - - - - - - - - -
-
-
- change_route -
- (SetRoute.srv) -
-
-
- - change_route... - - - - - - - - -
-
-
- route -
- (Route.msg) -
-
-
- - route... - - - - - - - - -
-
-
- state -
- (RouteState.msg) -
-
-
- - state... - - - - - - - - - - -
-
-
- behavior_path_planner -
-
-
- - behavior_path_planner - - - - - - - - -
-
-
- modified_goal -
- (with uuid) -
-
-
- - modified_goal... - - - - - - -
-
-
- route -
- (with uuid) -
-
-
- - route... - - - - - - - - - - -
-
-
- MRM -
-
-
- - MRM - - - - - - - - -
-
-
- T.B.D. -
- (Request MRM) -
-
-
- - T.B.D.... - - - - - - - - -
-
-
- clear_route -
- (ClearRoute.srv) -
-
-
- - clear_route... - - - - - - - - -
-
-
- clear_mrm_route -
-
-
- - clear_mrm_route - - - - - - -
-
-
- ADAPI -
-
-
- - ADAPI - - - - - - -
-
-
- planning -
-
-
- - planning - - - - - - -
-
-
- system -
-
-
- - system - - - - - - - - -
-
-
- set_mrm_route -
-
-
- - set_mrm_route - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 2dbefffe67738..3677e6c5fb075 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -39,12 +39,12 @@ class ObstacleAvoidancePlanner : public rclcpp::Node public: explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); - // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // NOTE: This is for the static_centerline_generator package which utilizes the following // instance. std::shared_ptr getMPTOptimizer() const { return mpt_optimizer_ptr_; } // private: -protected: // for the static_centerline_optimizer package +protected: // for the static_centerline_generator package // TODO(murooka) move this node to common class DrivingDirectionChecker { diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp index fb49a7fa446be..d119acf09b6ea 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -129,6 +129,8 @@ ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point); std::vector convertToReferencePoints( const std::vector & traj_points); +std::vector sanitizePoints(const std::vector & points); + void compensateLastPose( const PathPoint & last_path_point, std::vector & traj_points, const double delta_dist_threshold, const double delta_yaw_threshold); diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 7057a2885fd52..94dc62b1335d4 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -567,6 +567,9 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points = motion_utils::cropPoints( ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin, backward_traj_length + tmp_margin); + + // remove repeated points + ref_points = trajectory_utils::sanitizePoints(ref_points); SplineInterpolationPoints2d ref_points_spline(ref_points); ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_); @@ -586,6 +589,7 @@ std::vector MPTOptimizer::calcReferencePoints( // NOTE: This must be after backward cropping. // New start point may be added and resampled. Spline calculation is required. updateFixedPoint(ref_points); + ref_points = trajectory_utils::sanitizePoints(ref_points); ref_points_spline = SplineInterpolationPoints2d(ref_points); // 6. update bounds diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index 453d55bd7f770..651b11b28e8bc 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -79,6 +79,24 @@ std::vector convertToReferencePoints( return ref_points; } +std::vector sanitizePoints(const std::vector & points) +{ + std::vector output; + for (size_t i = 0; i < points.size(); i++) { + if (i > 0) { + const auto & current_pos = points.at(i).pose.position; + const auto & prev_pos = points.at(i - 1).pose.position; + if ( + std::fabs(current_pos.x - prev_pos.x) < 1e-6 && + std::fabs(current_pos.y - prev_pos.y) < 1e-6) { + continue; + } + } + output.push_back(points.at(i)); + } + return output; +} + void compensateLastPose( const PathPoint & last_path_point, std::vector & traj_points, const double delta_dist_threshold, const double delta_yaw_threshold) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index d1a9d0ff56b52..c368265ea0f66 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -21,6 +21,7 @@ #include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -138,14 +139,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr acc_sub_; - - // data for callback functions - PredictedObjects::ConstSharedPtr objects_ptr_{nullptr}; - Odometry::ConstSharedPtr ego_odom_ptr_{nullptr}; - AccelWithCovarianceStamped::ConstSharedPtr ego_accel_ptr_{nullptr}; + tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber objects_sub_{ + this, "~/input/objects"}; + tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ + this, "~/input/acceleration"}; // Vehicle Parameters VehicleInfo vehicle_info_; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 2d1b70d1745f9..a069b4b6f9395 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -357,15 +357,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & traj_sub_ = create_subscription( "~/input/trajectory", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); - objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{1}, - [this](const PredictedObjects::ConstSharedPtr msg) { objects_ptr_ = msg; }); - odom_sub_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, - [this](const Odometry::ConstSharedPtr msg) { ego_odom_ptr_ = msg; }); - acc_sub_ = create_subscription( - "~/input/acceleration", rclcpp::QoS{1}, - [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { ego_accel_ptr_ = msg; }); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -493,10 +484,15 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); + if ( + !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() || + !acc_sub_.updateLatestData()) { + return; + } + const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready - if (traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_) { + if (traj_points.empty()) { return; } @@ -609,11 +605,11 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( { stop_watch_.tic(__func__); - const auto obj_stamp = rclcpp::Time(objects_ptr_->header.stamp); + const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - for (const auto & predicted_object : objects_ptr_->objects) { + for (const auto & predicted_object : objects_sub_.getData().objects) { const auto & object_id = tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = @@ -631,7 +627,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, ego_odom_ptr_->pose.pose); + const size_t ego_idx = + ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); const auto ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); if (!ego_to_obstacle_distance) { @@ -727,7 +724,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); const auto decimated_traj_polys = - createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose); + createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -785,7 +782,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( slow_down_condition_counter_.removeCounterUnlessUpdated(); // Check target obstacles' consistency - checkConsistency(objects_ptr_->header.stamp, *objects_ptr_, stop_obstacles); + checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles); // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -807,7 +804,7 @@ std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints // trim trajectory const size_t ego_seg_idx = - ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_ptr_->pose.pose); + ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose); const size_t traj_start_point_idx = ego_seg_idx; const auto trimmed_traj_points = std::vector(traj_points.begin() + traj_start_point_idx, traj_points.end()); @@ -1191,7 +1188,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( // calculate collision points with trajectory with lateral stop margin const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop); + traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); @@ -1261,7 +1258,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, + traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; @@ -1424,8 +1421,8 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const { - const auto & ego_pose = ego_odom_ptr_->pose.pose; - const double ego_vel = ego_odom_ptr_->twist.twist.linear.x; + const auto & ego_pose = ego_odom_sub_.getData().pose.pose; + const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; const double time_to_reach_collision_point = [&]() { const double abs_ego_offset = is_driving_forward @@ -1457,9 +1454,9 @@ PlannerData ObstacleCruisePlannerNode::createPlannerData( PlannerData planner_data; planner_data.current_time = now(); planner_data.traj_points = traj_points; - planner_data.ego_pose = ego_odom_ptr_->pose.pose; - planner_data.ego_vel = ego_odom_ptr_->twist.twist.linear.x; - planner_data.ego_acc = ego_accel_ptr_->accel.accel.linear.x; + planner_data.ego_pose = ego_odom_sub_.getData().pose.pose; + planner_data.ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; + planner_data.ego_acc = acc_sub_.getData().accel.accel.linear.x; planner_data.is_driving_forward = is_driving_forward_; return planner_data; } diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index eb2d651cc967d..e0bdb326adb33 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -38,7 +38,7 @@ class ElasticBandSmoother : public rclcpp::Node public: explicit ElasticBandSmoother(const rclcpp::NodeOptions & node_options); - // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // NOTE: This is for the static_centerline_generator package which utilizes the following // instance. std::shared_ptr getElasticBandSmoother() const { return eb_path_smoother_ptr_; } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 83c814b70f0a5..ac8e472b2f943 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -354,6 +354,7 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getClosestLanelets(const geometry_msgs::msg::Pose & target_pose) const; private: // MUST diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 8cec5073efb6a..76763821998bd 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1708,7 +1708,7 @@ PathWithLaneId RouteHandler::getCenterLinePath( // append a point only when having one point so that yaw calculation would work if (reference_path.points.size() == 1) { - const lanelet::Id lane_id = static_cast(reference_path.points.front().lane_ids.front()); + const lanelet::Id lane_id = reference_path.points.front().lane_ids.front(); const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id); const auto point = reference_path.points.front().point.pose.position; const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point); @@ -2286,4 +2286,11 @@ bool RouteHandler::findDrivableLanePath( return drivable_lane_path_found; } +lanelet::ConstLanelets RouteHandler::getClosestLanelets( + const geometry_msgs::msg::Pose & target_pose) const +{ + lanelet::ConstLanelets target_lanelets; + lanelet::utils::query::getCurrentLanelets(road_lanelets_, target_pose, &target_lanelets); + return target_lanelets; +} } // namespace route_handler diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index a9002c8cf8a9f..50464f12114b0 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -37,7 +37,7 @@ class PathSampler : public rclcpp::Node public: explicit PathSampler(const rclcpp::NodeOptions & node_options); -protected: // for the static_centerline_optimizer package +protected: // for the static_centerline_generator package // argument variables vehicle_info_util::VehicleInfo vehicle_info_{}; mutable DebugData debug_data_{}; diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt similarity index 70% rename from planning/static_centerline_optimizer/CMakeLists.txt rename to planning/static_centerline_generator/CMakeLists.txt index 829c06c1fba12..991d12097cc08 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(static_centerline_optimizer) +project(static_centerline_generator) find_package(autoware_cmake REQUIRED) @@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces( - static_centerline_optimizer + static_centerline_generator "srv/LoadMap.srv" "srv/PlanRoute.srv" "srv/PlanPath.srv" @@ -21,16 +21,18 @@ autoware_package() ament_auto_add_executable(main src/main.cpp - src/static_centerline_optimizer_node.cpp + src/static_centerline_generator_node.cpp + src/centerline_source/optimization_trajectory_based_centerline.cpp + src/centerline_source/bag_ego_trajectory_based_centerline.cpp src/utils.cpp ) if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(main - static_centerline_optimizer "rosidl_typesupport_cpp") + static_centerline_generator "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target static_centerline_optimizer "rosidl_typesupport_cpp") + cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp") target_link_libraries(main "${cpp_typesupport_target}") endif() @@ -43,7 +45,7 @@ ament_auto_package( if(BUILD_TESTING) add_launch_test( - test/test_static_centerline_optimizer.test.py + test/test_static_centerline_generator.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/planning/static_centerline_optimizer/README.md b/planning/static_centerline_generator/README.md similarity index 93% rename from planning/static_centerline_optimizer/README.md rename to planning/static_centerline_generator/README.md index fd5a58ef816df..00572b754645c 100644 --- a/planning/static_centerline_optimizer/README.md +++ b/planning/static_centerline_generator/README.md @@ -1,4 +1,4 @@ -# Static Centerline Optimizer +# Static Centerline Generator ## Purpose @@ -34,7 +34,7 @@ We can run with the following command by designating `` ```sh -ros2 launch static_centerline_optimizer run_planning_server.launch.xml vehicle_model:= +ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:= ``` FYI, port ID of the http server is 4010 by default. @@ -50,7 +50,7 @@ The optimized centerline can be generated from the command line interface by des - `` ```sh -ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= +ros2 launch static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= ``` The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. diff --git a/planning/static_centerline_optimizer/config/common.param.yaml b/planning/static_centerline_generator/config/common.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/common.param.yaml rename to planning/static_centerline_generator/config/common.param.yaml diff --git a/planning/static_centerline_optimizer/config/nearest_search.param.yaml b/planning/static_centerline_generator/config/nearest_search.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/nearest_search.param.yaml rename to planning/static_centerline_generator/config/nearest_search.param.yaml diff --git a/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml b/planning/static_centerline_generator/config/static_centerline_generator.param.yaml similarity index 76% rename from planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml rename to planning/static_centerline_generator/config/static_centerline_generator.param.yaml index f62c34496e024..24a5536949479 100644 --- a/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml +++ b/planning/static_centerline_generator/config/static_centerline_generator.param.yaml @@ -2,3 +2,4 @@ ros__parameters: marker_color: ["FF0000", "00FF00", "0000FF"] marker_color_dist_thresh : [0.1, 0.2, 0.3] + output_trajectory_interval: 1.0 diff --git a/planning/static_centerline_optimizer/config/vehicle_info.param.yaml b/planning/static_centerline_generator/config/vehicle_info.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/vehicle_info.param.yaml rename to planning/static_centerline_generator/config/vehicle_info.param.yaml diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp new file mode 100644 index 0000000000000..30b2e55c36bba --- /dev/null +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -0,0 +1,27 @@ +// 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 STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_generator/type_alias.hpp" + +#include + +namespace static_centerline_generator +{ +std::vector generate_centerline_with_bag(rclcpp::Node & node); +} // namespace static_centerline_generator +#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp new file mode 100644 index 0000000000000..7e7cdea31e9f1 --- /dev/null +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -0,0 +1,43 @@ +// 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 STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT + +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_generator/type_alias.hpp" + +#include + +namespace static_centerline_generator +{ +class OptimizationTrajectoryBasedCenterline +{ +public: + OptimizationTrajectoryBasedCenterline() = default; + explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); + std::vector generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids); + +private: + std::vector optimize_trajectory(const Path & raw_path) const; + + rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; + rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; +}; +} // namespace static_centerline_generator +// clang-format off +#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +// clang-format on diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp new file mode 100644 index 0000000000000..c1d92c06a8e05 --- /dev/null +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -0,0 +1,119 @@ +// 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 STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "static_centerline_generator/srv/load_map.hpp" +#include "static_centerline_generator/srv/plan_path.hpp" +#include "static_centerline_generator/srv/plan_route.hpp" +#include "static_centerline_generator/type_alias.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include + +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/int32.hpp" + +#include +#include +#include +#include + +namespace static_centerline_generator +{ +using static_centerline_generator::srv::LoadMap; +using static_centerline_generator::srv::PlanPath; +using static_centerline_generator::srv::PlanRoute; + +struct CenterlineWithRoute +{ + std::vector centerline{}; + std::vector route_lane_ids{}; +}; + +class StaticCenterlineGeneratorNode : public rclcpp::Node +{ +public: + explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); + void run(); + +private: + // load map + void load_map(const std::string & lanelet2_input_file_path); + void on_load_map( + const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); + + // plan route + std::vector plan_route( + const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); + void on_plan_route( + const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); + + // plan centerline + CenterlineWithRoute generate_centerline_with_route(); + void on_plan_path( + const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); + + void update_centerline_range(const int traj_start_index, const int traj_end_index); + void evaluate( + const std::vector & route_lane_ids, + const std::vector & optimized_traj_points); + void save_map( + const std::string & lanelet2_output_file_path, + const CenterlineWithRoute & centerline_with_route); + + lanelet::LaneletMapPtr original_map_ptr_{nullptr}; + HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + std::shared_ptr route_handler_ptr_{nullptr}; + std::unique_ptr map_projector_{nullptr}; + + std::pair traj_range_indices_{0, 0}; + std::optional centerline_with_route_{std::nullopt}; + + enum class CenterlineSource { + OptimizationTrajectoryBase = 0, + BagEgoTrajectoryBase, + }; + CenterlineSource centerline_source_; + OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; + + // publisher + rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; + rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; + + // subscriber + rclcpp::Subscription::SharedPtr sub_traj_start_index_; + rclcpp::Subscription::SharedPtr sub_traj_end_index_; + rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; + + // service + rclcpp::Service::SharedPtr srv_load_map_; + rclcpp::Service::SharedPtr srv_plan_route_; + rclcpp::Service::SharedPtr srv_plan_path_; + + // callback group for service + rclcpp::CallbackGroup::SharedPtr callback_group_; + + // vehicle info + vehicle_info_util::VehicleInfo vehicle_info_; +}; +} // namespace static_centerline_generator +#endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp similarity index 87% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 1a700d36deaaf..2dcb9cbbd099f 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -11,8 +11,8 @@ // 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 STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ +#ifndef STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#define STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include "autoware_planning_msgs/msg/lanelet_route.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace static_centerline_optimizer +namespace static_centerline_generator { using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -41,6 +41,6 @@ using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator -#endif // STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ +#endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp similarity index 78% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index 8c8c346557fce..c8cf8f8b90590 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_ +#ifndef STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ +#define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ #include "route_handler/route_handler.hpp" -#include "static_centerline_optimizer/type_alias.hpp" +#include "static_centerline_generator/type_alias.hpp" #include @@ -24,10 +24,15 @@ #include #include -namespace static_centerline_optimizer +namespace static_centerline_generator { namespace utils { +rclcpp::QoS create_transient_local_qos(); + +lanelet::ConstLanelets get_lanelets_from_ids( + const RouteHandler & route_handler, const std::vector & lane_ids); + geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id); @@ -48,6 +53,6 @@ MarkerArray create_distance_text_marker( const geometry_msgs::msg::Pose & pose, const double dist, const std::array & marker_color, const rclcpp::Time & now, const size_t idx); } // namespace utils -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator -#endif // STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_ +#endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ diff --git a/planning/static_centerline_optimizer/launch/run_planning_server.launch.xml b/planning/static_centerline_generator/launch/run_planning_server.launch.xml similarity index 54% rename from planning/static_centerline_optimizer/launch/run_planning_server.launch.xml rename to planning/static_centerline_generator/launch/run_planning_server.launch.xml index 3a8b88a021dc0..1493078317458 100644 --- a/planning/static_centerline_optimizer/launch/run_planning_server.launch.xml +++ b/planning/static_centerline_generator/launch/run_planning_server.launch.xml @@ -2,11 +2,11 @@ - + - + diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml similarity index 85% rename from planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml rename to planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index 37a9abc47bfeb..ae71b0ae6e925 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -5,6 +5,8 @@ + + @@ -53,9 +55,10 @@ - + + @@ -76,9 +79,11 @@ - + + + - + diff --git a/planning/static_centerline_optimizer/media/rviz.png b/planning/static_centerline_generator/media/rviz.png similarity index 100% rename from planning/static_centerline_optimizer/media/rviz.png rename to planning/static_centerline_generator/media/rviz.png diff --git a/planning/static_centerline_optimizer/media/unsafe_footprints.png b/planning/static_centerline_generator/media/unsafe_footprints.png similarity index 100% rename from planning/static_centerline_optimizer/media/unsafe_footprints.png rename to planning/static_centerline_generator/media/unsafe_footprints.png diff --git a/planning/static_centerline_optimizer/msg/PointsWithLaneId.msg b/planning/static_centerline_generator/msg/PointsWithLaneId.msg similarity index 100% rename from planning/static_centerline_optimizer/msg/PointsWithLaneId.msg rename to planning/static_centerline_generator/msg/PointsWithLaneId.msg diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_generator/package.xml similarity index 91% rename from planning/static_centerline_optimizer/package.xml rename to planning/static_centerline_generator/package.xml index 363c88bebf0ea..6573f6e439c43 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -1,9 +1,9 @@ - static_centerline_optimizer + static_centerline_generator 0.1.0 - The static_centerline_optimizer package + The static_centerline_generator package Takayuki Murooka Kosuke Takeuchi Apache License 2.0 @@ -19,11 +19,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs behavior_path_planner_common + geography_utils geometry_msgs global_parameter_loader interpolation lanelet2_extension map_loader + map_projection_loader mission_planner motion_utils obstacle_avoidance_planner diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_generator/rviz/static_centerline_generator.rviz similarity index 98% rename from planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz rename to planning/static_centerline_generator/rviz/static_centerline_generator.rviz index 1e7573d561ba1..62b4c9b75ec87 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz +++ b/planning/static_centerline_generator/rviz/static_centerline_generator.rviz @@ -130,7 +130,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/input_centerline + Value: /static_centerline_generator/input_centerline Value: true View Drivable Area: Alpha: 0.9990000128746033 @@ -179,7 +179,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/output_centerline + Value: /static_centerline_generator/output_centerline Value: true View Footprint: Alpha: 1 @@ -222,7 +222,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/debug/raw_centerline + Value: /static_centerline_generator/debug/raw_centerline Value: false View Drivable Area: Alpha: 0.9990000128746033 @@ -268,7 +268,7 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/debug/unsafe_footprints + Value: /static_centerline_generator/debug/unsafe_footprints Value: true Enabled: false Name: debug diff --git a/planning/static_centerline_optimizer/scripts/app.py b/planning/static_centerline_generator/scripts/app.py similarity index 92% rename from planning/static_centerline_optimizer/scripts/app.py rename to planning/static_centerline_generator/scripts/app.py index 90610755e828a..c1cb0473da040 100755 --- a/planning/static_centerline_optimizer/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -23,12 +23,12 @@ from flask_cors import CORS import rclpy from rclpy.node import Node -from static_centerline_optimizer.srv import LoadMap -from static_centerline_optimizer.srv import PlanPath -from static_centerline_optimizer.srv import PlanRoute +from static_centerline_generator.srv import LoadMap +from static_centerline_generator.srv import PlanPath +from static_centerline_generator.srv import PlanRoute rclpy.init() -node = Node("static_centerline_optimizer_http_server") +node = Node("static_centerline_generator_http_server") app = Flask(__name__) CORS(app) @@ -51,7 +51,7 @@ def get_map(): map_id = map_uuid # create client - cli = create_client(LoadMap, "/planning/static_centerline_optimizer/load_map") + cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") # request map loading req = LoadMap.Request(map=data["map"]) @@ -85,7 +85,7 @@ def post_planned_route(): print("map_id is not correct.") # create client - cli = create_client(PlanRoute, "/planning/static_centerline_optimizer/plan_route") + cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") # request route planning req = PlanRoute.Request( @@ -123,7 +123,7 @@ def post_planned_path(): print("map_id is not correct.") # create client - cli = create_client(PlanPath, "/planning/static_centerline_optimizer/plan_path") + cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") # request path planning route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] diff --git a/planning/static_centerline_generator/scripts/centerline_updater_helper.py b/planning/static_centerline_generator/scripts/centerline_updater_helper.py new file mode 100755 index 0000000000000..fec3d21d20bec --- /dev/null +++ b/planning/static_centerline_generator/scripts/centerline_updater_helper.py @@ -0,0 +1,198 @@ +#!/bin/env python3 + +# 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. + + +import sys +import time + +from PyQt5 import QtCore +from PyQt5.QtWidgets import QApplication +from PyQt5.QtWidgets import QGridLayout +from PyQt5.QtWidgets import QMainWindow +from PyQt5.QtWidgets import QPushButton +from PyQt5.QtWidgets import QSizePolicy +from PyQt5.QtWidgets import QSlider +from PyQt5.QtWidgets import QWidget +from autoware_auto_planning_msgs.msg import Trajectory +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from std_msgs.msg import Bool +from std_msgs.msg import Int32 + + +class CenterlineUpdaterWidget(QMainWindow): + def __init__(self): + super(self.__class__, self).__init__() + self.setupUI() + + def setupUI(self): + self.setObjectName("MainWindow") + self.resize(480, 120) + self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) + + central_widget = QWidget(self) + central_widget.setObjectName("central_widget") + + self.grid_layout = QGridLayout(central_widget) + self.grid_layout.setContentsMargins(10, 10, 10, 10) + self.grid_layout.setObjectName("grid_layout") + + # button to update the trajectory's start and end index + self.update_button = QPushButton("update slider") + self.update_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.update_button.clicked.connect(self.onUpdateButton) + + # button to reset the trajectory's start and end index + self.reset_button = QPushButton("reset") + self.reset_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.reset_button.clicked.connect(self.onResetButton) + + # button to save map + self.save_map_button = QPushButton("save map") + self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + + # slide of the trajectory's start and end index + self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal) + self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal) + self.min_traj_start_index = 0 + self.max_traj_end_index = None + + # set layout + self.grid_layout.addWidget(self.update_button, 1, 0, 1, -1) + self.grid_layout.addWidget(self.reset_button, 2, 0, 1, -1) + self.grid_layout.addWidget(self.save_map_button, 3, 0, 1, -1) + self.grid_layout.addWidget(self.traj_start_index_slider, 4, 0, 1, -1) + self.grid_layout.addWidget(self.traj_end_index_slider, 5, 0, 1, -1) + self.setCentralWidget(central_widget) + + def initWithEndIndex(self, max_traj_end_index): + self.max_traj_end_index = max_traj_end_index + + # initialize slider + self.displayed_min_traj_start_index = self.min_traj_start_index + self.displayed_max_traj_end_index = self.max_traj_end_index + self.initializeSlider() + + def initializeSlider(self, move_value_to_end=True): + self.traj_start_index_slider.setMinimum(0) + self.traj_end_index_slider.setMinimum(0) + self.traj_start_index_slider.setMaximum( + self.displayed_max_traj_end_index - self.displayed_min_traj_start_index + ) + self.traj_end_index_slider.setMaximum( + self.displayed_max_traj_end_index - self.displayed_min_traj_start_index + ) + + if move_value_to_end: + self.traj_start_index_slider.setValue(0) + self.traj_end_index_slider.setValue(self.traj_end_index_slider.maximum()) + + def onResetButton(self, event): + current_traj_start_index = self.displayed_min_traj_start_index + current_traj_end_index = self.displayed_max_traj_end_index + + self.displayed_min_traj_start_index = self.min_traj_start_index + self.displayed_max_traj_end_index = self.max_traj_end_index + + self.initializeSlider(False) + self.traj_start_index_slider.setValue(current_traj_start_index) + if ( + current_traj_start_index == self.min_traj_start_index + and current_traj_end_index == self.max_traj_end_index + ): + self.traj_end_index_slider.setValue(self.displayed_max_traj_end_index) + else: + self.traj_end_index_slider.setValue( + current_traj_start_index + self.traj_end_index_slider.value() + ) + + def onUpdateButton(self, event): + current_traj_start_index = self.getCurrentStartIndex() + current_traj_end_index = self.getCurrentEndIndex() + + self.displayed_min_traj_start_index = current_traj_start_index + self.displayed_max_traj_end_index = current_traj_end_index + + self.initializeSlider() + + def getCurrentStartIndex(self): + return self.displayed_min_traj_start_index + self.traj_start_index_slider.value() + + def getCurrentEndIndex(self): + return self.displayed_min_traj_start_index + self.traj_end_index_slider.value() + + +class CenterlineUpdaterHelper(Node): + def __init__(self): + super().__init__("centerline_updater_helper") + # Qt + self.widget = CenterlineUpdaterWidget() + self.widget.show() + self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed) + self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged) + self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged) + + # ROS + self.pub_save_map = self.create_publisher(Bool, "~/save_map", 1) + self.pub_traj_start_index = self.create_publisher(Int32, "~/traj_start_index", 1) + self.pub_traj_end_index = self.create_publisher(Int32, "~/traj_end_index", 1) + + transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.sub_whole_centerline = self.create_subscription( + Trajectory, + "/static_centerline_generator/output_whole_centerline", + self.onWholeCenterline, + transient_local_qos, + ) + + while self.widget.max_traj_end_index is None: + rclpy.spin_once(self, timeout_sec=0.01) + time.sleep(0.1) + + def onWholeCenterline(self, whole_centerline): + self.widget.initWithEndIndex(len(whole_centerline.points) - 1) + + def onSaveMapButtonPushed(self, event): + msg = Bool() + msg.data = True + self.pub_save_map.publish(msg) + + def onStartIndexChanged(self, event): + msg = Int32() + msg.data = self.widget.getCurrentStartIndex() + self.pub_traj_start_index.publish(msg) + + def onEndIndexChanged(self, event): + msg = Int32() + msg.data = self.widget.getCurrentEndIndex() + self.pub_traj_end_index.publish(msg) + + +def main(args=None): + app = QApplication(sys.argv) + + rclpy.init(args=args) + node = CenterlineUpdaterHelper() + + while True: + app.processEvents() + rclpy.spin_once(node, timeout_sec=0.01) + + +if __name__ == "__main__": + main() diff --git a/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh new file mode 100755 index 0000000000000..30ed667dd3732 --- /dev/null +++ b/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +ros2 launch static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus diff --git a/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh new file mode 100755 index 0000000000000..809bbb46a179e --- /dev/null +++ b/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +ros2 launch static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp new file mode 100644 index 0000000000000..4e541b1aff527 --- /dev/null +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -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. + +#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" + +#include "rclcpp/serialization.hpp" +#include "rosbag2_cpp/reader.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" + +#include + +namespace static_centerline_generator +{ +std::vector generate_centerline_with_bag(rclcpp::Node & node) +{ + const auto bag_filename = node.declare_parameter("bag_filename"); + + // open rosbag + rosbag2_cpp::Reader bag_reader; + bag_reader.open(bag_filename); + + // extract 2D position of ego's trajectory from rosbag + rclcpp::Serialization bag_serialization; + std::vector centerline_traj_points; + while (bag_reader.has_next()) { + const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next(); + + if (msg->topic_name != "/localization/kinematic_state") { + continue; + } + + rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); + const auto ros_msg = std::make_shared(); + + bag_serialization.deserialize_message(&serialized_msg, ros_msg.get()); + + if (!centerline_traj_points.empty()) { + constexpr double epsilon = 1e-1; + if ( + std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < + epsilon && + std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < + epsilon) { + continue; + } + } + TrajectoryPoint centerline_traj_point; + centerline_traj_point.pose.position = ros_msg->pose.pose.position; + centerline_traj_points.push_back(centerline_traj_point); + } + + RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag."); + + // calculate rough orientation of centerline trajectory points + for (size_t i = 0; i < centerline_traj_points.size(); ++i) { + if (i == centerline_traj_points.size() - 1) { + if (i != 0) { + centerline_traj_points.at(i).pose.orientation = + centerline_traj_points.at(i - 1).pose.orientation; + } + } else { + const double yaw_angle = tier4_autoware_utils::calcAzimuthAngle( + centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); + centerline_traj_points.at(i).pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw_angle); + } + } + + return centerline_traj_points; +} +} // namespace static_centerline_generator diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp new file mode 100644 index 0000000000000..7980ae4e8d2d7 --- /dev/null +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -0,0 +1,183 @@ +// 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 "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" + +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/conversion.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "path_smoother/elastic_band_smoother.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator/utils.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" + +namespace static_centerline_generator +{ +namespace +{ +rclcpp::NodeOptions create_node_options() +{ + return rclcpp::NodeOptions{}; +} + +Path convert_to_path(const PathWithLaneId & path_with_lane_id) +{ + Path path; + path.header = path_with_lane_id.header; + path.left_bound = path_with_lane_id.left_bound; + path.right_bound = path_with_lane_id.right_bound; + for (const auto & point : path_with_lane_id.points) { + path.points.push_back(point.point); + } + + return path; +} + +Trajectory convert_to_trajectory(const Path & path) +{ + Trajectory traj; + for (const auto & point : path.points) { + TrajectoryPoint traj_point; + traj_point.pose = point.pose; + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + + traj.points.push_back(traj_point); + } + return traj; +} +} // namespace + +OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node) +{ + pub_raw_path_with_lane_id_ = + node.create_publisher("input_centerline", utils::create_transient_local_qos()); + pub_raw_path_ = + node.create_publisher("debug/raw_centerline", utils::create_transient_local_qos()); +} + +std::vector +OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids) +{ + const auto route_lanelets = utils::get_lanelets_from_ids(route_handler, route_lane_ids); + + // optimize centerline inside the lane + const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); + + // get ego nearest search parameters and resample interval in behavior_path_planner + const double ego_nearest_dist_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + const double ego_nearest_yaw_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + const double behavior_path_interval = + tier4_autoware_utils::getOrDeclareParameter(node, "output_path_interval"); + const double behavior_vel_interval = + tier4_autoware_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); + + // extract path with lane id from lanelets + const auto raw_path_with_lane_id = [&]() { + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + }(); + pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); + RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); + + // convert path with lane id to path + const auto raw_path = [&]() { + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + }(); + pub_raw_path_->publish(raw_path); + RCLCPP_INFO(node.get_logger(), "Converted to path and published."); + + // smooth trajectory and road collision avoidance + const auto optimized_traj_points = optimize_trajectory(raw_path); + RCLCPP_INFO( + node.get_logger(), + "Smoothed trajectory and made it collision free with the road and published."); + + return optimized_traj_points; +} + +std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( + const Path & raw_path) const +{ + // convert to trajectory points + const auto raw_traj_points = [&]() { + const auto raw_traj = convert_to_trajectory(raw_path); + return motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); + + // create an instance of elastic band and model predictive trajectory. + const auto eb_path_smoother_ptr = + path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); + const auto mpt_optimizer_ptr = + obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); + + // NOTE: The optimization is executed every valid_optimized_traj_points_num points. + constexpr int valid_optimized_traj_points_num = 10; + const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; + + // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use + // warm start. + constexpr int num_initial_optimization = 2; + + std::vector whole_optimized_traj_points; + for (int virtual_ego_pose_idx = -num_initial_optimization; + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { + // calculate virtual ego pose for the optimization + constexpr int virtual_ego_pose_offset_idx = 1; + const auto virtual_ego_pose = + raw_traj_points + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; + + // smooth trajectory by elastic band in the path_smoother package + const auto smoothed_traj_points = + eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); + + // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // package + const obstacle_avoidance_planner::PlannerData planner_data{ + raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, + virtual_ego_pose}; + const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); + + // connect the previously and currently optimized trajectory points + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), + whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + break; + } + } + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + return whole_optimized_traj_points; +} +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/src/main.cpp b/planning/static_centerline_generator/src/main.cpp similarity index 86% rename from planning/static_centerline_optimizer/src/main.cpp rename to planning/static_centerline_generator/src/main.cpp index 96c139fd7ff93..981cf54fc9274 100644 --- a/planning/static_centerline_optimizer/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" int main(int argc, char * argv[]) { @@ -21,7 +21,7 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; auto node = - std::make_shared(node_options); + std::make_shared(node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp similarity index 56% rename from planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp rename to planning/static_centerline_generator/src/static_centerline_generator_node.cpp index d98341ecb2e23..ec7d6278e346d 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -12,25 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/node.hpp" -#include "path_smoother/elastic_band_smoother.hpp" -#include "static_centerline_optimizer/msg/points_with_lane_id.hpp" -#include "static_centerline_optimizer/type_alias.hpp" -#include "static_centerline_optimizer/utils.hpp" +#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "static_centerline_generator/msg/points_with_lane_id.hpp" +#include "static_centerline_generator/type_alias.hpp" +#include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include #include #include #include +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/int32.hpp" #include #include @@ -41,57 +46,17 @@ #include #include +#include #include #include #include #include #include -namespace static_centerline_optimizer +namespace static_centerline_generator { namespace { -Path convert_to_path(const PathWithLaneId & path_with_lane_id) -{ - Path path; - path.header = path_with_lane_id.header; - path.left_bound = path_with_lane_id.left_bound; - path.right_bound = path_with_lane_id.right_bound; - for (const auto & point : path_with_lane_id.points) { - path.points.push_back(point.point); - } - - return path; -} - -Trajectory convert_to_trajectory(const Path & path) -{ - Trajectory traj; - for (const auto & point : path.points) { - TrajectoryPoint traj_point; - traj_point.pose = point.pose; - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - - traj.points.push_back(traj_point); - } - return traj; -} - -[[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route( - const RouteHandler & route_handler, const LaneletRoute & route) -{ - lanelet::ConstLanelets lanelets; - for (const auto & segment : route.segments) { - const auto & target_lanelet_id = segment.preferred_primitive.id; - const auto target_lanelet = route_handler.getLaneletsFromId(target_lanelet_id); - lanelets.push_back(target_lanelet); - } - - return lanelets; -} - std::vector get_lane_ids_from_route(const LaneletRoute & route) { std::vector lane_ids; @@ -103,28 +68,7 @@ std::vector get_lane_ids_from_route(const LaneletRoute & route) return lane_ids; } -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids) -{ - lanelet::ConstLanelets lanelets; - for (const lanelet::Id lane_id : lane_ids) { - const auto lanelet = route_handler.getLaneletsFromId(lane_id); - lanelets.push_back(lanelet); - } - return lanelets; -} - -rclcpp::NodeOptions create_node_options() -{ - return rclcpp::NodeOptions{}; -} - -rclcpp::QoS create_transient_local_qos() -{ - return rclcpp::QoS{1}.transient_local(); -} - -lanelet::BasicPoint2d convertToLaneletPoint(const geometry_msgs::msg::Point & geom_point) +lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & geom_point) { lanelet::BasicPoint2d point(geom_point.x, geom_point.y); return point; @@ -169,7 +113,7 @@ geometry_msgs::msg::Pose get_text_pose( return tier4_autoware_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } -std::array convertHexStringToDecimal(const std::string & hex_str_color) +std::array convert_hex_string_to_decimal(const std::string & hex_str_color) { unsigned int hex_int_color; std::istringstream iss(hex_str_color); @@ -202,78 +146,210 @@ std::vector check_lanelet_connection( return unconnected_lane_ids; } + +std_msgs::msg::Header create_header(const rclcpp::Time & now) +{ + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = now; + return header; +} + +std::vector resample_trajectory_points( + const std::vector & input_traj_points, const double resample_interval) +{ + // resample and calculate trajectory points' orientation + const auto input_traj = motion_utils::convertToTrajectory(input_traj_points); + auto resampled_input_traj = motion_utils::resampleTrajectory(input_traj, resample_interval); + return motion_utils::convertToTrajectoryPointArray(resampled_input_traj); +} } // namespace -StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( +StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( const rclcpp::NodeOptions & node_options) -: Node("static_centerline_optimizer", node_options) +: Node("static_centerline_generator", node_options) { // publishers - pub_map_bin_ = create_publisher("lanelet2_map_topic", create_transient_local_qos()); - pub_raw_path_with_lane_id_ = - create_publisher("input_centerline", create_transient_local_qos()); - pub_raw_path_ = create_publisher("debug/raw_centerline", create_transient_local_qos()); - pub_optimized_centerline_ = - create_publisher("output_centerline", create_transient_local_qos()); + pub_map_bin_ = + create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); + pub_whole_centerline_ = + create_publisher("output_whole_centerline", utils::create_transient_local_qos()); + pub_centerline_ = + create_publisher("output_centerline", utils::create_transient_local_qos()); // debug publishers pub_debug_unsafe_footprints_ = - create_publisher("debug/unsafe_footprints", create_transient_local_qos()); + create_publisher("debug/unsafe_footprints", utils::create_transient_local_qos()); + + // subscribers + sub_traj_start_index_ = create_subscription( + "/centerline_updater_helper/traj_start_index", rclcpp::QoS{1}, + [this](const std_msgs::msg::Int32 & msg) { + update_centerline_range(msg.data, traj_range_indices_.second); + }); + sub_traj_end_index_ = create_subscription( + "/centerline_updater_helper/traj_end_index", rclcpp::QoS{1}, + [this](const std_msgs::msg::Int32 & msg) { + update_centerline_range(traj_range_indices_.first, msg.data); + }); + sub_save_map_ = create_subscription( + "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { + const auto lanelet2_output_file_path = + tier4_autoware_utils::getOrDeclareParameter( + *this, "lanelet2_output_file_path"); + if (!centerline_with_route_ || msg.data) { + const auto & c = *centerline_with_route_; + const auto selected_centerline = std::vector( + c.centerline.begin() + traj_range_indices_.first, + c.centerline.begin() + traj_range_indices_.second + 1); + save_map( + lanelet2_output_file_path, CenterlineWithRoute{selected_centerline, c.route_lane_ids}); + } + }); + sub_traj_resample_interval_ = create_subscription( + "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) { + // TODO(murooka) + }); // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_load_map_ = create_service( - "/planning/static_centerline_optimizer/load_map", + "/planning/static_centerline_generator/load_map", std::bind( - &StaticCenterlineOptimizerNode::on_load_map, this, std::placeholders::_1, + &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); srv_plan_route_ = create_service( - "/planning/static_centerline_optimizer/plan_route", + "/planning/static_centerline_generator/plan_route", std::bind( - &StaticCenterlineOptimizerNode::on_plan_route, this, std::placeholders::_1, + &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); srv_plan_path_ = create_service( - "/planning/static_centerline_optimizer/plan_path", + "/planning/static_centerline_generator/plan_path", std::bind( - &StaticCenterlineOptimizerNode::on_plan_path, this, std::placeholders::_1, + &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); // vehicle info vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + centerline_source_ = [&]() { + const auto centerline_source_param = declare_parameter("centerline_source"); + if (centerline_source_param == "optimization_trajectory_base") { + optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); + return CenterlineSource::OptimizationTrajectoryBase; + } else if (centerline_source_param == "bag_ego_trajectory_base") { + return CenterlineSource::BagEgoTrajectoryBase; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_generator."); + }(); +} + +void StaticCenterlineGeneratorNode::update_centerline_range( + const int traj_start_index, const int traj_end_index) +{ + if (!centerline_with_route_ || traj_range_indices_.second + 1 < traj_start_index) { + return; + } + + traj_range_indices_ = std::make_pair(traj_start_index, traj_end_index); + + const auto & centerline = centerline_with_route_->centerline; + const auto selected_centerline = std::vector( + centerline.begin() + traj_range_indices_.first, + centerline.begin() + traj_range_indices_.second + 1); + + pub_centerline_->publish( + motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); } -void StaticCenterlineOptimizerNode::run() +void StaticCenterlineGeneratorNode::run() { // declare planning setting parameters const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); const auto lanelet2_output_file_path = declare_parameter("lanelet2_output_file_path"); - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); // process load_map(lanelet2_input_file_path); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - const auto optimized_traj_points = plan_path(route_lane_ids); - save_map(lanelet2_output_file_path, route_lane_ids, optimized_traj_points); + const auto centerline_with_route = generate_centerline_with_route(); + traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1); + save_map(lanelet2_output_file_path, centerline_with_route); + + centerline_with_route_ = centerline_with_route; +} + +CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_route() +{ + if (!route_handler_ptr_) { + RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); + return CenterlineWithRoute{}; + } + + // generate centerline with route + auto centerline_with_route = [&]() { + if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { + const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); + const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto optimized_centerline = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + *this, *route_handler_ptr_, route_lane_ids); + return CenterlineWithRoute{optimized_centerline, route_lane_ids}; + } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + const auto bag_centerline = generate_centerline_with_bag(*this); + const auto start_lanelets = + route_handler_ptr_->getClosestLanelets(bag_centerline.front().pose); + const auto end_lanelets = route_handler_ptr_->getClosestLanelets(bag_centerline.back().pose); + if (start_lanelets.empty() || end_lanelets.empty()) { + RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); + return CenterlineWithRoute{}; + } + + const lanelet::Id start_lanelet_id = start_lanelets.front().id(); + const lanelet::Id end_lanelet_id = end_lanelets.front().id(); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + + return CenterlineWithRoute{bag_centerline, route_lane_ids}; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_generator."); + }(); + + // resample + const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); + centerline_with_route.centerline = + resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); + + pub_whole_centerline_->publish(motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); + + pub_centerline_->publish(motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); + + return centerline_with_route; } -void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_file_path) +void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map - tier4_map_msgs::msg::MapProjectorInfo map_projector_info; - map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); const auto map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); if (!map_ptr) { return nullptr; } + // NOTE: generate map projector for lanelet::write(). + // Without this, lat/lon of the generated LL2 map will be wrong. + map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); + // NOTE: The original map is stored here since the various ids in the lanelet map will change // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. original_map_ptr_ = @@ -305,7 +381,7 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_ route_handler_ptr_->setMap(*map_bin_ptr_); } -void StaticCenterlineOptimizerNode::on_load_map( +void StaticCenterlineGeneratorNode::on_load_map( const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) { const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; @@ -326,7 +402,7 @@ void StaticCenterlineOptimizerNode::on_load_map( response->message = "InvalidMapFormat"; } -std::vector StaticCenterlineOptimizerNode::plan_route( +std::vector StaticCenterlineGeneratorNode::plan_route( const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) { if (!map_bin_ptr_ || !route_handler_ptr_) { @@ -351,7 +427,7 @@ std::vector StaticCenterlineOptimizerNode::plan_route( plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); // initialize mission_planner - auto node = rclcpp::Node("po"); + auto node = rclcpp::Node("mission_planner"); mission_planner->initialize(&node, map_bin_ptr_); // plan route @@ -366,7 +442,7 @@ std::vector StaticCenterlineOptimizerNode::plan_route( return route_lane_ids; } -void StaticCenterlineOptimizerNode::on_plan_route( +void StaticCenterlineGeneratorNode::on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) { if (!map_bin_ptr_ || !route_handler_ptr_) { @@ -380,7 +456,7 @@ void StaticCenterlineOptimizerNode::on_plan_route( // plan route const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // extract lane ids std::vector lane_ids; @@ -399,132 +475,7 @@ void StaticCenterlineOptimizerNode::on_plan_route( response->lane_ids = lane_ids; } -std::vector StaticCenterlineOptimizerNode::plan_path( - const std::vector & route_lane_ids) -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); - return std::vector{}; - } - - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // optimize centerline inside the lane - const auto start_center_pose = - utils::get_center_pose(*route_handler_ptr_, route_lane_ids.front()); - - // get ego nearest search parameters and resample interval in behavior_path_planner - const double ego_nearest_dist_threshold = - has_parameter("ego_nearest_dist_threshold") - ? get_parameter("ego_nearest_dist_threshold").as_double() - : declare_parameter("ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = - has_parameter("ego_nearest_yaw_threshold") - ? get_parameter("ego_nearest_yaw_threshold").as_double() - : declare_parameter("ego_nearest_yaw_threshold"); - const double behavior_path_interval = has_parameter("output_path_interval") - ? get_parameter("output_path_interval").as_double() - : declare_parameter("output_path_interval"); - const double behavior_vel_interval = - has_parameter("behavior_output_path_interval") - ? get_parameter("behavior_output_path_interval").as_double() - : declare_parameter("behavior_output_path_interval"); - - // extract path with lane id from lanelets - const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); - }(); - pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); - RCLCPP_INFO(get_logger(), "Calculated raw path with lane id and published."); - - // convert path with lane id to path - const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); - pub_raw_path_->publish(raw_path); - RCLCPP_INFO(get_logger(), "Converted to path and published."); - - // smooth trajectory and road collision avoidance - const auto optimized_traj_points = optimize_trajectory(raw_path); - pub_optimized_centerline_->publish( - motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header)); - RCLCPP_INFO( - get_logger(), "Smoothed trajectory and made it collision free with the road and published."); - - return optimized_traj_points; -} - -std::vector StaticCenterlineOptimizerNode::optimize_trajectory( - const Path & raw_path) const -{ - // convert to trajectory points - const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); - - // create an instance of elastic band and model predictive trajectory. - const auto eb_path_smoother_ptr = - path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); - const auto mpt_optimizer_ptr = - obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); - - // NOTE: The optimization is executed every valid_optimized_traj_points_num points. - constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; - - // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use - // warm start. - constexpr int num_initial_optimization = 2; - - std::vector whole_optimized_traj_points; - for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { - // calculate virtual ego pose for the optimization - constexpr int virtual_ego_pose_offset_idx = 1; - const auto virtual_ego_pose = - raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; - - // smooth trajectory by elastic band in the path_smoother package - const auto smoothed_traj_points = - eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - - // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner - // package - const obstacle_avoidance_planner::PlannerData planner_data{ - raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, - virtual_ego_pose}; - const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); - - // connect the previously and currently optimized trajectory points - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - break; - } - } - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - return whole_optimized_traj_points; -} - -void StaticCenterlineOptimizerNode::on_plan_path( +void StaticCenterlineGeneratorNode::on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) { if (!route_handler_ptr_) { @@ -535,7 +486,7 @@ void StaticCenterlineOptimizerNode::on_plan_path( // get lanelets from route lane ids const auto route_lane_ids = request->route; - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // check if input route lanelets are connected to each other. const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); @@ -547,7 +498,9 @@ void StaticCenterlineOptimizerNode::on_plan_path( } // plan path - const auto optimized_traj_points = plan_path(route_lane_ids); + const auto optimized_traj_points = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + *this, *route_handler_ptr_, route_lane_ids); // check calculation result if (optimized_traj_points.empty()) { @@ -566,8 +519,8 @@ void StaticCenterlineOptimizerNode::on_plan_path( std::vector current_lanelet_points; // check if target point is inside the lanelet - while ( - lanelet::geometry::inside(lanelet, convertToLaneletPoint(target_traj_point->pose.position))) { + while (lanelet::geometry::inside( + lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { // memorize points inside the lanelet current_lanelet_points.push_back(target_traj_point->pose.position); target_traj_point++; @@ -580,7 +533,7 @@ void StaticCenterlineOptimizerNode::on_plan_path( if (!current_lanelet_points.empty()) { // register points with lane_id - static_centerline_optimizer::msg::PointsWithLaneId points_with_lane_id; + static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; points_with_lane_id.lane_id = lanelet.id(); points_with_lane_id.points = current_lanelet_points; response->points_with_lane_ids.push_back(points_with_lane_id); @@ -595,23 +548,20 @@ void StaticCenterlineOptimizerNode::on_plan_path( response->message = ""; } -void StaticCenterlineOptimizerNode::evaluate( +void StaticCenterlineGeneratorNode::evaluate( const std::vector & route_lane_ids, const std::vector & optimized_traj_points) { - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - const auto dist_thresh_vec = - has_parameter("marker_color_dist_thresh") - ? get_parameter("marker_color_dist_thresh").as_double_array() - : declare_parameter>("marker_color_dist_thresh"); - const auto marker_color_vec = has_parameter("marker_color") - ? get_parameter("marker_color").as_string_array() - : declare_parameter>("marker_color"); + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto dist_thresh_vec = tier4_autoware_utils::getOrDeclareParameter>( + *this, "marker_color_dist_thresh"); + const auto marker_color_vec = + tier4_autoware_utils::getOrDeclareParameter>(*this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); if (dist < dist_thresh) { - return convertHexStringToDecimal(marker_color_vec.at(i)); + return convert_hex_string_to_decimal(marker_color_vec.at(i)); } } return boost::none; @@ -668,22 +618,22 @@ void StaticCenterlineOptimizerNode::evaluate( RCLCPP_INFO(get_logger(), "Minimum distance to road is %f [m]", min_dist); } -void StaticCenterlineOptimizerNode::save_map( - const std::string & lanelet2_output_file_path, const std::vector & route_lane_ids, - const std::vector & optimized_traj_points) +void StaticCenterlineGeneratorNode::save_map( + const std::string & lanelet2_output_file_path, const CenterlineWithRoute & centerline_with_route) { if (!route_handler_ptr_) { return; } - const auto route_lanelets = get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); + const auto & c = centerline_with_route; + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); // update centerline in map - utils::update_centerline(*route_handler_ptr_, route_lanelets, optimized_traj_points); + utils::update_centerline(*route_handler_ptr_, route_lanelets, c.centerline); RCLCPP_INFO(get_logger(), "Updated centerline in map."); // save map with modified center line - lanelet::write(lanelet2_output_file_path, *original_map_ptr_); + lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_); RCLCPP_INFO(get_logger(), "Saved map."); } -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp similarity index 93% rename from planning/static_centerline_optimizer/src/utils.cpp rename to planning/static_centerline_generator/src/utils.cpp index f8cc0953a6dbd..ddfd3e11036ce 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_optimizer/utils.hpp" +#include "static_centerline_generator/utils.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -21,7 +21,7 @@ #include #include -namespace static_centerline_optimizer +namespace static_centerline_generator { namespace { @@ -45,6 +45,22 @@ lanelet::Point3d createPoint3d(const double x, const double y, const double z = namespace utils { +rclcpp::QoS create_transient_local_qos() +{ + return rclcpp::QoS{1}.transient_local(); +} + +lanelet::ConstLanelets get_lanelets_from_ids( + const RouteHandler & route_handler, const std::vector & lane_ids) +{ + lanelet::ConstLanelets lanelets; + for (const lanelet::Id lane_id : lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + lanelets.push_back(lanelet); + } + return lanelets; +} + geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id) { @@ -212,4 +228,4 @@ MarkerArray create_distance_text_marker( return marker_array; } } // namespace utils -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/srv/LoadMap.srv b/planning/static_centerline_generator/srv/LoadMap.srv similarity index 100% rename from planning/static_centerline_optimizer/srv/LoadMap.srv rename to planning/static_centerline_generator/srv/LoadMap.srv diff --git a/planning/static_centerline_optimizer/srv/PlanPath.srv b/planning/static_centerline_generator/srv/PlanPath.srv similarity index 55% rename from planning/static_centerline_optimizer/srv/PlanPath.srv rename to planning/static_centerline_generator/srv/PlanPath.srv index 644a4ea013557..7d823b4eccbf9 100644 --- a/planning/static_centerline_optimizer/srv/PlanPath.srv +++ b/planning/static_centerline_generator/srv/PlanPath.srv @@ -1,6 +1,6 @@ uint32 map_id int64[] route --- -static_centerline_optimizer/PointsWithLaneId[] points_with_lane_ids +static_centerline_generator/PointsWithLaneId[] points_with_lane_ids string message int64[] unconnected_lane_ids diff --git a/planning/static_centerline_optimizer/srv/PlanRoute.srv b/planning/static_centerline_generator/srv/PlanRoute.srv similarity index 100% rename from planning/static_centerline_optimizer/srv/PlanRoute.srv rename to planning/static_centerline_generator/srv/PlanRoute.srv diff --git a/planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 b/planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 new file mode 100644 index 0000000000000..0883307acbae0 Binary files /dev/null and b/planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 differ diff --git a/planning/static_centerline_optimizer/test/data/lanelet2_map.osm b/planning/static_centerline_generator/test/data/lanelet2_map.osm similarity index 100% rename from planning/static_centerline_optimizer/test/data/lanelet2_map.osm rename to planning/static_centerline_generator/test/data/lanelet2_map.osm diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py similarity index 82% rename from planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py rename to planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 36bdfc732ed79..29ee49a11e3b3 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,17 +28,18 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("static_centerline_optimizer"), "test/data/lanelet2_map.osm" + get_package_share_directory("static_centerline_generator"), "test/data/lanelet2_map.osm" ) - static_centerline_optimizer_node = Node( - package="static_centerline_optimizer", + static_centerline_generator_node = Node( + package="static_centerline_generator", executable="main", output="screen", parameters=[ {"lanelet2_map_path": lanelet2_map_path}, {"run_background": False}, {"rviz": False}, + {"centerline_source": "optimization_trajectory_base"}, {"lanelet2_input_file_path": lanelet2_map_path}, {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, {"start_lanelet_id": 215}, @@ -49,8 +50,8 @@ def generate_test_description(): "mission_planner.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_optimizer"), - "config/static_centerline_optimizer.param.yaml", + get_package_share_directory("static_centerline_generator"), + "config/static_centerline_generator.param.yaml", ), os.path.join( get_package_share_directory("behavior_path_planner"), @@ -73,15 +74,15 @@ def generate_test_description(): "config/lanelet2_map_loader.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_optimizer"), + get_package_share_directory("static_centerline_generator"), "config/common.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_optimizer"), + get_package_share_directory("static_centerline_generator"), "config/nearest_search.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_optimizer"), + get_package_share_directory("static_centerline_generator"), "config/vehicle_info.param.yaml", ), ], @@ -92,8 +93,8 @@ def generate_test_description(): return ( LaunchDescription( [ - static_centerline_optimizer_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + static_centerline_generator_node, + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp deleted file mode 100644 index fa7043de42f5c..0000000000000 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ /dev/null @@ -1,89 +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 STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "static_centerline_optimizer/srv/load_map.hpp" -#include "static_centerline_optimizer/srv/plan_path.hpp" -#include "static_centerline_optimizer/srv/plan_route.hpp" -#include "static_centerline_optimizer/type_alias.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include -#include - -namespace static_centerline_optimizer -{ -using static_centerline_optimizer::srv::LoadMap; -using static_centerline_optimizer::srv::PlanPath; -using static_centerline_optimizer::srv::PlanRoute; - -class StaticCenterlineOptimizerNode : public rclcpp::Node -{ -public: - explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options); - void run(); - -private: - // load map - void load_map(const std::string & lanelet2_input_file_path); - void on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); - - // plan route - std::vector plan_route( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); - void on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); - - // plan path - std::vector plan_path(const std::vector & route_lane_ids); - std::vector optimize_trajectory(const Path & raw_path) const; - void on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); - - void evaluate( - const std::vector & route_lane_ids, - const std::vector & optimized_traj_points); - void save_map( - const std::string & lanelet2_output_file_path, const std::vector & route_lane_ids, - const std::vector & optimized_traj_points); - - lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; - std::shared_ptr route_handler_ptr_{nullptr}; - - // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; - rclcpp::Publisher::SharedPtr pub_optimized_centerline_{nullptr}; - - // service - rclcpp::Service::SharedPtr srv_load_map_; - rclcpp::Service::SharedPtr srv_plan_route_; - rclcpp::Service::SharedPtr srv_plan_path_; - - // callback group for service - rclcpp::CallbackGroup::SharedPtr callback_group_; - - // vehicle info - vehicle_info_util::VehicleInfo vehicle_info_; -}; -} // namespace static_centerline_optimizer -#endif // STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/scripts/tmp/run.sh b/planning/static_centerline_optimizer/scripts/tmp/run.sh deleted file mode 100755 index 07857a9d923a1..0000000000000 --- a/planning/static_centerline_optimizer/scripts/tmp/run.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/AutonomousDrivingScenarios/map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 7aae6c937f4a9..12b034043894e 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -83,7 +83,12 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.1)); input_offset_ = declare_parameter("input_offset", std::vector{}); - if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { + + // If input_offset_ is not defined, set all offsets to 0 + if (input_offset_.empty()) { + input_offset_.resize(input_topics_.size(), 0.0); + RCLCPP_INFO(get_logger(), "Input offset is not defined. Set all offsets to 0.0."); + } else if (input_topics_.size() != input_offset_.size()) { RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); return; } diff --git a/simulator/fault_injection/README.md b/simulator/fault_injection/README.md index cbac41ebfb640..cb3d6fea96a7a 100644 --- a/simulator/fault_injection/README.md +++ b/simulator/fault_injection/README.md @@ -27,7 +27,9 @@ launch_test test/test_fault_injection_node.test.py ### Output -None. +| Name | Type | Description | +| -------------- | --------------------------------------- | ----------------- | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Dummy diagnostics | ## Parameters diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/fault_injection/config/fault_injection.param.yaml index ac02442d70b4d..e9e90a336348e 100644 --- a/simulator/fault_injection/config/fault_injection.param.yaml +++ b/simulator/fault_injection/config/fault_injection.param.yaml @@ -1,37 +1,37 @@ /**: ros__parameters: event_diag_list: - vehicle_is_out_of_lane: "lane_departure" - trajectory_deviation_is_high: "trajectory_deviation" - localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_error_ellipse" - map_version_is_different: "map_version" - trajectory_is_invalid: "trajectory_point_validation" - cpu_temperature_is_high: "CPU Temperature" - cpu_usage_is_high: "CPU Usage" - cpu_is_in_thermal_throttling: "CPU Thermal Throttling" - storage_temperature_is_high: "HDD Temperature" - storage_usage_is_high: "HDD Usage" - network_usage_is_high: "Network Usage" - clock_error_is_large: "NTP Offset" - gpu_temperature_is_high: "GPU Temperature" - gpu_usage_is_high: "GPU Usage" - gpu_memory_usage_is_high: "GPU Memory Usage" - gpu_is_in_thermal_throttling: "GPU Thermal Throttling" - driving_recorder_storage_error: "driving_recorder" - debug_data_logger_storage_error: "bagpacker" - emergency_stop_operation: "emergency_stop_operation" - vehicle_error_occurred: "vehicle_errors" - vehicle_ecu_connection_is_lost: "can_bus_connection" - obstacle_crash_sensor_is_activated: "obstacle_crash" + vehicle_is_out_of_lane: ": lane_departure" + trajectory_deviation_is_high: ": trajectory_deviation" + localization_matching_score_is_low: ": ndt_scan_matcher" + localization_accuracy_is_low: ": localization_error_ellipse" + map_version_is_different: ": map_version" + trajectory_is_invalid: ": trajectory_point_validation" + cpu_temperature_is_high: ": CPU Temperature" + cpu_usage_is_high: ": CPU Usage" + cpu_is_in_thermal_throttling: ": CPU Thermal Throttling" + storage_temperature_is_high: ": HDD Temperature" + storage_usage_is_high: ": HDD Usage" + network_usage_is_high: ": Network Usage" + clock_error_is_large: ": NTP Offset" + gpu_temperature_is_high: ": GPU Temperature" + gpu_usage_is_high: ": GPU Usage" + gpu_memory_usage_is_high: ": GPU Memory Usage" + gpu_is_in_thermal_throttling: ": GPU Thermal Throttling" + driving_recorder_storage_error: ": driving_recorder" + debug_data_logger_storage_error: ": bagpacker" + emergency_stop_operation: ": emergency_stop_operation" + vehicle_error_occurred: ": vehicle_errors" + vehicle_ecu_connection_is_lost: ": can_bus_connection" + obstacle_crash_sensor_is_activated: ": obstacle_crash" /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat" - /control/autonomous_driving/node_alive_monitoring: "control_topic_status" + /control/autonomous_driving/node_alive_monitoring: ": control_topic_status" /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat" - /localization/node_alive_monitoring: "localization_topic_status" - /map/node_alive_monitoring: "map_topic_status" - /planning/node_alive_monitoring: "planning_topic_status" - /sensing/lidar/node_alive_monitoring: "lidar_topic_status" - /sensing/imu/node_alive_monitoring: "imu_connection" - /sensing/gnss/node_alive_monitoring: "gnss_connection" - /system/node_alive_monitoring: "system_topic_status" - /vehicle/node_alive_monitoring: "vehicle_topic_status" + /localization/node_alive_monitoring: ": localization_topic_status" + /map/node_alive_monitoring: ": map_topic_status" + /planning/node_alive_monitoring: ": planning_topic_status" + /sensing/lidar/node_alive_monitoring: ": lidar_topic_status" + /sensing/imu/node_alive_monitoring: ": imu_connection" + /sensing/gnss/node_alive_monitoring: ": gnss_connection" + /system/node_alive_monitoring: ": system_topic_status" + /vehicle/node_alive_monitoring: ": vehicle_topic_status" diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp b/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp new file mode 100644 index 0000000000000..948c2b45f6615 --- /dev/null +++ b/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp @@ -0,0 +1,244 @@ +// 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. + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ +#define FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ + +#include + +#include +#include +#include +#include + +namespace fault_injection +{ +class FaultInjectionDiagUpdater : public diagnostic_updater::DiagnosticTaskVector +{ +public: + template + explicit FaultInjectionDiagUpdater(NodeT node, double period = 1.0) + : FaultInjectionDiagUpdater( + node->get_node_base_interface(), node->get_node_clock_interface(), + node->get_node_logging_interface(), node->get_node_timers_interface(), + node->get_node_topics_interface(), period) + { + } + + FaultInjectionDiagUpdater( + const std::shared_ptr & base_interface, + const std::shared_ptr & clock_interface, + const std::shared_ptr & logging_interface, + std::shared_ptr timers_interface, + std::shared_ptr topics_interface, + double period = 1.0) + : base_interface_(base_interface), + timers_interface_(std::move(timers_interface)), + clock_(clock_interface->get_clock()), + period_(rclcpp::Duration::from_nanoseconds(static_cast(period * 1e9))), + publisher_(rclcpp::create_publisher( + topics_interface, "/diagnostics", 1)), + logger_(logging_interface->get_logger()), + node_name_(base_interface->get_name()) + { + reset_timer(); + } + + /** + * \brief Returns the interval between updates. + */ + [[nodiscard]] auto get_period() const { return period_; } + + /** + * \brief Sets the period as a rclcpp::Duration + */ + void set_period(const rclcpp::Duration & period) + { + period_ = period; + reset_timer(); + } + + /** + * \brief Sets the period given a value in seconds + */ + void set_period(double period) + { + set_period(rclcpp::Duration::from_nanoseconds(static_cast(period * 1e9))); + } + + /** + * \brief Forces to send out an update for all known DiagnosticStatus. + */ + void force_update() { update(); } + + /** + * \brief Output a message on all the known DiagnosticStatus. + * + * Useful if something drastic is happening such as shutdown or a + * self-test. + * + * \param lvl Level of the diagnostic being output. + * + * \param msg Status message to output. + */ + void broadcast(int lvl, const std::string & msg) + { + std::vector status_vec; + + const std::vector & tasks = getTasks(); + for (const auto & task : tasks) { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = task.getName(); + status.summary(lvl, msg); + + status_vec.push_back(status); + } + + publish(status_vec); + } + + void set_hardware_id_f(const char * format, ...) + { + va_list va; + const int k_buffer_size = 1000; + char buff[1000]; // @todo This could be done more elegantly. + va_start(va, format); + if (vsnprintf(buff, k_buffer_size, format, va) >= k_buffer_size) { + RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf."); + } + hardware_id_ = std::string(buff); + va_end(va); + } + + void set_hardware_id(const std::string & hardware_id) { hardware_id_ = hardware_id; } + +private: + void reset_timer() + { + update_timer_ = rclcpp::create_timer( + base_interface_, timers_interface_, clock_, period_, + std::bind(&FaultInjectionDiagUpdater::update, this)); + } + + /** + * \brief Causes the diagnostics to update if the inter-update interval + * has been exceeded. + */ + void update() + { + if (rclcpp::ok()) { + std::vector status_vec; + + std::unique_lock lock( + lock_); // Make sure no adds happen while we are processing here. + const std::vector & tasks = getTasks(); + for (const auto & task : tasks) { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = task.getName(); + status.level = 2; + status.message = "No message was set"; + status.hardware_id = hardware_id_; + + task.run(status); + + status_vec.push_back(status); + } + + publish(status_vec); + } + } + + /** + * Publishes a single diagnostic status. + */ + void publish(diagnostic_msgs::msg::DiagnosticStatus & stat) + { + std::vector status_vec; + status_vec.push_back(stat); + publish(status_vec); + } + + /** + * Publishes a vector of diagnostic statuses. + */ + void publish(std::vector & status_vec) + { + diagnostic_msgs::msg::DiagnosticArray msg; + msg.status = status_vec; + msg.header.stamp = clock_->now(); + publisher_->publish(msg); + } + + /** + * Causes a placeholder DiagnosticStatus to be published as soon as a + * diagnostic task is added to the Updater. + */ + void addedTaskCallback(DiagnosticTaskInternal & task) override + { + diagnostic_updater::DiagnosticStatusWrapper stat; + stat.name = task.getName(); + stat.summary(0, "Node starting up"); + publish(stat); + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Duration period_; + rclcpp::TimerBase::SharedPtr update_timer_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Logger logger_; + + std::string hardware_id_; + std::string node_name_; +}; +} // namespace fault_injection + +#endif // FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp b/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp index 0f578ff343a17..999d18b38c7b0 100644 --- a/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp +++ b/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp @@ -16,8 +16,8 @@ #define FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ #include "fault_injection/diagnostic_storage.hpp" +#include "fault_injection/fault_injection_diag_updater.hpp" -#include #include #include @@ -36,23 +36,15 @@ class FaultInjectionNode : public rclcpp::Node private: // Subscribers - void onSimulationEvents(const SimulationEvents::ConstSharedPtr msg); + void on_simulation_events(const SimulationEvents::ConstSharedPtr msg); rclcpp::Subscription::SharedPtr sub_simulation_events_; - // Parameter Server - rcl_interfaces::msg::SetParametersResult onSetParam( - const std::vector & params); - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - void updateEventDiag( + void update_event_diag( diagnostic_updater::DiagnosticStatusWrapper & wrap, const std::string & event_name); - void addDiag( - const diagnostic_msgs::msg::DiagnosticStatus & status, diagnostic_updater::Updater & updater); - - std::vector readEventDiagList(); + std::vector read_event_diag_list(); - diagnostic_updater::Updater updater_; + FaultInjectionDiagUpdater updater_; DiagnosticStorage diagnostic_storage_; }; diff --git a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp index 9b86fa2de4294..7b8f87400beab 100644 --- a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -43,30 +43,26 @@ using rosidl_generator_traits::to_yaml; FaultInjectionNode::FaultInjectionNode(rclcpp::NodeOptions node_options) : Node("fault_injection", node_options.automatically_declare_parameters_from_overrides(true)), - updater_(this) + updater_(this, 0.05) { - updater_.setHardwareID("fault_injection"); + updater_.set_hardware_id("fault_injection"); using std::placeholders::_1; - // Parameter Server - set_param_res_ = - this->add_on_set_parameters_callback(std::bind(&FaultInjectionNode::onSetParam, this, _1)); - // Subscriber sub_simulation_events_ = this->create_subscription( "~/input/simulation_events", rclcpp::QoS{rclcpp::KeepLast(10)}, - std::bind(&FaultInjectionNode::onSimulationEvents, this, _1)); + std::bind(&FaultInjectionNode::on_simulation_events, this, _1)); // Load all config - for (const auto & diag : readEventDiagList()) { + for (const auto & diag : read_event_diag_list()) { diagnostic_storage_.registerEvent(diag); updater_.add( - diag.diag_name, std::bind(&FaultInjectionNode::updateEventDiag, this, _1, diag.sim_name)); + diag.diag_name, std::bind(&FaultInjectionNode::update_event_diag, this, _1, diag.sim_name)); } } -void FaultInjectionNode::onSimulationEvents(const SimulationEvents::ConstSharedPtr msg) +void FaultInjectionNode::on_simulation_events(const SimulationEvents::ConstSharedPtr msg) { RCLCPP_DEBUG(this->get_logger(), "Received data: %s", to_yaml(*msg).c_str()); for (const auto & event : msg->fault_injection_events) { @@ -76,7 +72,7 @@ void FaultInjectionNode::onSimulationEvents(const SimulationEvents::ConstSharedP } } -void FaultInjectionNode::updateEventDiag( +void FaultInjectionNode::update_event_diag( diagnostic_updater::DiagnosticStatusWrapper & wrap, const std::string & event_name) { const auto diag = diagnostic_storage_.getDiag(event_name); @@ -86,30 +82,7 @@ void FaultInjectionNode::updateEventDiag( wrap.hardware_id = diag.hardware_id; } -rcl_interfaces::msg::SetParametersResult FaultInjectionNode::onSetParam( - const std::vector & params) -{ - rcl_interfaces::msg::SetParametersResult result; - - RCLCPP_DEBUG(this->get_logger(), "call onSetParam"); - - try { - double value; - if (tier4_autoware_utils::updateParam(params, "diagnostic_updater.period", value)) { - updater_.setPeriod(value); - } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - return result; - } - - result.successful = true; - result.reason = "success"; - return result; -} - -std::vector FaultInjectionNode::readEventDiagList() +std::vector FaultInjectionNode::read_event_diag_list() { // Expected parameter name is "event_diag_list.param_name". // In this case, depth is 2. diff --git a/simulator/fault_injection/test/config/test_event_diag.param.yaml b/simulator/fault_injection/test/config/test_event_diag.param.yaml index 83f58fcc455ee..d9006d239b924 100644 --- a/simulator/fault_injection/test/config/test_event_diag.param.yaml +++ b/simulator/fault_injection/test/config/test_event_diag.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: event_diag_list: - cpu_temperature: "CPU Temperature" - cpu_usage: "CPU Usage" - cpu_load_average: "CPU Load Average" + cpu_temperature: ": CPU Temperature" + cpu_usage: ": CPU Usage" + cpu_load_average: ": CPU Load Average" diff --git a/simulator/fault_injection/test/test_fault_injection_node.test.py b/simulator/fault_injection/test/test_fault_injection_node.test.py index 4556aaaca8d80..1437d69b1c91b 100644 --- a/simulator/fault_injection/test/test_fault_injection_node.test.py +++ b/simulator/fault_injection/test/test_fault_injection_node.test.py @@ -167,11 +167,11 @@ def test_receive_multiple_message_simultaneously(self): # Verify the latest message for stat in msg_buffer[-1].status: - if stat.name == "fault_injection: CPU Load Average": + if stat.name == ": CPU Load Average": self.assertEqual(stat.level, DiagnosticStatus.OK) - elif stat.name == "fault_injection: CPU Temperature": + elif stat.name == ": CPU Temperature": self.assertEqual(stat.level, DiagnosticStatus.ERROR) - elif stat.name == "fault_injection: CPU Usage": + elif stat.name == ": CPU Usage": self.assertEqual(stat.level, DiagnosticStatus.ERROR) else: self.fail(f"Unexpected status name: {stat.name}") diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index 9ae7a384927b9..082aa85cba97a 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_auto_system_msgs diagnostic_msgs rclcpp diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index e1da4f463f06d..58282306491dd 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -32,10 +32,10 @@ enum class HazardLevel { NF, SF, LF, SPF }; struct TempNode { const DiagnosticNode & node; - bool is_auto_tree; + bool is_root_tree; }; -HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_level) +HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel root_mode_level) { // Convert the level according to the table below. // The Level other than auto mode is considered OK. @@ -49,7 +49,7 @@ HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_le // | STALE | SF | LF | SPF | SPF | // |-------|-------------------------------| - const auto root_level = node.is_auto_tree ? auto_mode_level : DiagnosticStatus::OK; + const auto root_level = node.is_root_tree ? root_mode_level : DiagnosticStatus::OK; const auto node_level = node.node.status.level; if (node_level == DiagnosticStatus::OK) { @@ -67,20 +67,21 @@ HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_le return HazardLevel::SPF; } -void set_auto_tree(std::vector & nodes, int index) +void set_root_tree(std::vector & nodes, int index) { TempNode & node = nodes[index]; - if (node.is_auto_tree) { + if (node.is_root_tree) { return; } - node.is_auto_tree = true; + node.is_root_tree = true; for (const auto & link : node.node.links) { - set_auto_tree(nodes, link.index); + set_root_tree(nodes, link.index); } } -HazardStatusStamped convert_hazard_diagnostics(const DiagnosticGraph & graph) +HazardStatusStamped convert_hazard_diagnostics( + const DiagnosticGraph & graph, const std::string & root, bool ignore) { // Create temporary tree for conversion. std::vector nodes; @@ -90,19 +91,21 @@ HazardStatusStamped convert_hazard_diagnostics(const DiagnosticGraph & graph) } // Mark nodes included in the auto mode tree. - DiagnosticLevel auto_mode_level = DiagnosticStatus::STALE; - for (size_t index = 0; index < nodes.size(); ++index) { - const auto & status = nodes[index].node.status; - if (status.name == "/autoware/modes/autonomous") { - set_auto_tree(nodes, index); - auto_mode_level = status.level; + DiagnosticLevel root_mode_level = DiagnosticStatus::STALE; + if (!root.empty() && !ignore) { + for (size_t index = 0; index < nodes.size(); ++index) { + const auto & status = nodes[index].node.status; + if (status.name == root) { + set_root_tree(nodes, index); + root_mode_level = status.level; + } } } // Calculate hazard level from node level and root level. HazardStatusStamped hazard; for (const auto & node : nodes) { - switch (get_hazard_level(node, auto_mode_level)) { + switch (get_hazard_level(node, root_mode_level)) { case HazardLevel::NF: hazard.status.diag_no_fault.push_back(node.node.status); break; @@ -131,6 +134,22 @@ Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", op sub_graph_ = create_subscription( "~/diagnostics_graph", rclcpp::QoS(3), std::bind(&Converter::on_graph, this, std::placeholders::_1)); + sub_state_ = create_subscription( + "/autoware/state", rclcpp::QoS(1), + std::bind(&Converter::on_state, this, std::placeholders::_1)); + sub_mode_ = create_subscription( + "/system/operation_mode/state", rclcpp::QoS(1).transient_local(), + std::bind(&Converter::on_mode, this, std::placeholders::_1)); +} + +void Converter::on_state(const AutowareState::ConstSharedPtr msg) +{ + state_ = msg; +} + +void Converter::on_mode(const OperationMode::ConstSharedPtr msg) +{ + mode_ = msg; } void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg) @@ -148,7 +167,29 @@ void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg) return HazardStatus::NO_FAULT; }; - HazardStatusStamped hazard = convert_hazard_diagnostics(*msg); + const auto is_ignore = [this]() { + if (mode_ && state_) { + if (mode_->mode == OperationMode::AUTONOMOUS || mode_->mode == OperationMode::STOP) { + if (state_->state == AutowareState::WAITING_FOR_ROUTE) return true; + if (state_->state == AutowareState::PLANNING) return true; + } + if (state_->state == AutowareState::INITIALIZING) return true; + if (state_->state == AutowareState::FINALIZING) return true; + } + return false; + }; + + const auto get_root = [this]() { + if (mode_) { + if (mode_->mode == OperationMode::STOP) return "/autoware/modes/stop"; + if (mode_->mode == OperationMode::AUTONOMOUS) return "/autoware/modes/autonomous"; + if (mode_->mode == OperationMode::LOCAL) return "/autoware/modes/local"; + if (mode_->mode == OperationMode::REMOTE) return "/autoware/modes/remote"; + } + return ""; + }; + + HazardStatusStamped hazard = convert_hazard_diagnostics(*msg, get_root(), is_ignore()); hazard.stamp = msg->stamp; hazard.status.level = get_system_level(hazard.status); hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT; diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index 90c6f6e42bf85..04e84cfb3c0c4 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -17,6 +17,8 @@ #include +#include +#include #include #include @@ -33,11 +35,20 @@ class Converter : public rclcpp::Node explicit Converter(const rclcpp::NodeOptions & options); private: + using AutowareState = autoware_auto_system_msgs::msg::AutowareState; + using OperationMode = autoware_adapi_v1_msgs::msg::OperationModeState; using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph; using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped; + rclcpp::Subscription::SharedPtr sub_state_; + rclcpp::Subscription::SharedPtr sub_mode_; rclcpp::Subscription::SharedPtr sub_graph_; rclcpp::Publisher::SharedPtr pub_hazard_; + void on_state(const AutowareState::ConstSharedPtr msg); + void on_mode(const OperationMode::ConstSharedPtr msg); void on_graph(const DiagnosticGraph::ConstSharedPtr msg); + + AutowareState::ConstSharedPtr state_; + OperationMode::ConstSharedPtr mode_; }; } // namespace hazard_status_converter