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/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/evaluator/perception_online_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt index f9cc0f4fa256c..e1cd0546ea94a 100644 --- a/evaluator/perception_online_evaluator/CMakeLists.txt +++ b/evaluator/perception_online_evaluator/CMakeLists.txt @@ -31,6 +31,7 @@ target_link_libraries(${PROJECT_NAME}_node glog::glog) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_perception_online_evaluator_node.cpp + TIMEOUT 180 ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}_node diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index c0e2a516cf948..4fcd4fdf9cc84 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -11,6 +11,58 @@ 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. + +### Predicted Path Deviation / Predicted Path Deviation Variance + +Compare the predicted path of past objects with their actual traveled path to determine the deviation. For each object, calculate the mean distance between the predicted path points and the corresponding points on the actual path, up to the specified time step. In other words, this calculates the Average Displacement Error (ADE). The target object to be evaluated is the object from $T_N$ seconds ago, where $T_N$ is the maximum value of the prediction time horizon $[T_1, T_2, ..., T_N]$. + +![path_deviation_each_object](./images/path_deviation_each_object.drawio.svg) + +$$ +\begin{align} +n_{points} = T / dt \\ +ADE = \Sigma_{i=1}^{n_{points}} d_i / n_{points}　\\ +Var = \Sigma_{i=1}^{n_{points}} (d_i - ADE)^2 / n_{points} +\end{align} +$$ + +- $n_{points}$ : Number of points in the predicted path +- $T$ : Time horizon for prediction evaluation. +- $dt$ : Time interval of the predicted path +- $d_i$ : Distance between the predicted path and the actual traveled path at path point $i$ +- $ADE$ : Mean deviation of the predicted path for the target object. +- $Var$ : Variance of the predicted path deviation for the target object. + +The final predicted path deviation metrics are calculated by averaging the mean deviation of the predicted path for all objects of the same class, and then calculating the mean, maximum, and minimum values of the mean deviation. + +![path_deviation](./images/path_deviation.drawio.svg) + +$$ +\begin{align} +ADE_{mean} = \Sigma_{j=1}^{n_{objects}} ADE_j / n_{objects} \\ +ADE_{max} = max(ADE_j) \\ +ADE_{min} = min(ADE_j) +\end{align} +$$ + +$$ +\begin{align} +Var_{mean} = \Sigma_{j=1}^{n_{objects}} Var_j / n_{objects} \\ +Var_{max} = max(Var_j) \\ +Var_{min} = min(Var_j) +\end{align} +$$ + +- $n_{objects}$ : Number of objects +- $ADE_{mean}$ : Mean deviation of the predicted path through all objects +- $ADE_{max}$ : Maximum deviation of the predicted path through all objects +- $ADE_{min}$ : Minimum deviation of the predicted path through all objects +- $Var_{mean}$ : Mean variance of the predicted path deviation through all objects +- $Var_{max}$ : Maximum variance of the predicted path deviation through all objects +- $Var_{min}$ : Minimum variance of the predicted path deviation through all objects + +The actual metric name is determined by the object class and time horizon. For example, `predicted_path_deviation_variance_CAR_5.00` ## Inputs / Outputs diff --git a/evaluator/perception_online_evaluator/images/path_deviation.drawio.svg b/evaluator/perception_online_evaluator/images/path_deviation.drawio.svg new file mode 100644 index 0000000000000..0ff07f2808ea4 --- /dev/null +++ b/evaluator/perception_online_evaluator/images/path_deviation.drawio.svg @@ -0,0 +1,534 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `\object_1` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `\object_2` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `\object_j` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `ADE_1` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `ADE_2` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `ADE_j` + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg b/evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg new file mode 100644 index 0000000000000..39a69b5f198e9 --- /dev/null +++ b/evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg @@ -0,0 +1,1208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ past predicted path +
+
+
+ + past predicted path + + + + + + + + + + +
+
+
+ dt +
+
+
+ + dt + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + = dt * k +
+
+
+ + `T_k` = dt... + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + = dt * n +
+
+
+ + `T_n` = dt... + + + + + + + + + + + + + + + + +
+
+
+ actual traveled path +
+
+
+ + actual traveled path + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `d_1` + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `d_k` + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `d_n` + + + + + + + + + + +
+
+
+ object at + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + object at `T_{now} - T_N` + + + + + + + + + + +
+
+
+ object at + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + object at `T_{now}` + + + + + + + + + + +
+
+
+ prediction_time_horizons = + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ s.t. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + prediction_time_horizons = `[T_1, T_2, ... , T... + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `ADE_{T_k} = 1/k \Sigma_1^k d_i` + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `ADE_{T_n} = 1/n \Sigma_1^n d_i` + + + + + + + + + + + + + + + + + + + + +
+
+
+ dt * 2 +
+
+
+ + dt * 2 + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `d_2` + + + + + diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index f5dee74828424..e11ae89e48106 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -37,6 +37,12 @@ enum class Metric { using MetricStatMap = std::unordered_map>; +struct PredictedPathDeviationMetrics +{ + Stat mean; + Stat variance; +}; + static const std::unordered_map str_to_metric = { {"lateral_deviation", Metric::lateral_deviation}, {"yaw_deviation", Metric::yaw_deviation}, diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index f6566326ba979..ee0a1eac73762 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -126,7 +126,7 @@ class MetricsCalculator MetricStatMap calcLateralDeviationMetrics(const ClassObjectsMap & class_objects_map) const; MetricStatMap calcYawDeviationMetrics(const ClassObjectsMap & class_objects_map) const; MetricStatMap calcPredictedPathDeviationMetrics(const ClassObjectsMap & class_objects_map) const; - Stat calcPredictedPathDeviationMetrics( + PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 20d8c6d570489..c31a7f22d16c4 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -152,11 +152,17 @@ std::optional MetricsCalculator::getClosestObjectIterato std::optional MetricsCalculator::getObjectByStamp( const std::string uuid, const rclcpp::Time stamp) const { + constexpr double eps = 0.01; + constexpr double close_time_threshold = 0.1; + const auto obj_it_opt = getClosestObjectIterator(uuid, stamp); if (obj_it_opt.has_value()) { const auto it = obj_it_opt.value(); - if (it->first == getClosestStamp(stamp)) { - return it->second; + if (std::abs((it->first - getClosestStamp(stamp)).seconds()) < eps) { + const double time_diff = std::abs((it->first - stamp).seconds()); + if (time_diff < close_time_threshold) { + return it->second; + } } } return std::nullopt; @@ -256,31 +262,34 @@ MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics( MetricStatMap metric_stat_map{}; for (const auto & [label, objects] : class_objects_map) { for (const double time_horizon : time_horizons) { - const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon); + const auto metrics = calcPredictedPathDeviationMetrics(objects, time_horizon); std::ostringstream stream; stream << std::fixed << std::setprecision(2) << time_horizon; std::string time_horizon_str = stream.str(); metric_stat_map - ["predicted_path_deviation_" + convertLabelToString(label) + "_" + time_horizon_str] = stat; + ["predicted_path_deviation_" + convertLabelToString(label) + "_" + time_horizon_str] = + metrics.mean; + metric_stat_map + ["predicted_path_deviation_variance_" + convertLabelToString(label) + "_" + + time_horizon_str] = metrics.variance; } } return metric_stat_map; } -Stat MetricsCalculator::calcPredictedPathDeviationMetrics( +PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const { - // For each object, select the predicted path that is closest to the history path and store the - // distance to the history path + // Step 1: For each object and its predicted paths, calculate the deviation between each predicted + // path pose and the corresponding historical path pose. Store the deviations in + // deviation_map_for_paths. std::unordered_map>> deviation_map_for_paths; - // For debugging. Save the pairs of predicted path pose and history path pose. - // Visualize the correspondence in rviz from the node. + + // For debugging: Save the pairs of predicted path pose and history path pose. std::unordered_map>> debug_predicted_path_pairs_map; - // Find the corresponding pose in the history path for each pose of the predicted path of each - // object, and record the distances const auto stamp = objects.header.stamp; for (const auto & object : objects.objects) { const auto uuid = tier4_autoware_utils::toHexString(object.object_id); @@ -309,30 +318,36 @@ Stat MetricsCalculator::calcPredictedPathDeviationMetrics( const double distance = tier4_autoware_utils::calcDistance2d(p.position, history_pose.position); deviation_map_for_paths[uuid][i].push_back(distance); - // debug + + // Save debug information debug_predicted_path_pairs_map[path_id].push_back(std::make_pair(p, history_pose)); } } } - // Select the predicted path with the smallest deviation for each object + // Step 2: For each object, select the predicted path with the smallest mean deviation. + // Store the selected path's deviations in deviation_map_for_objects. std::unordered_map> deviation_map_for_objects; for (const auto & [uuid, deviation_map] : deviation_map_for_paths) { - size_t min_deviation_index = 0; - double min_sum_deviation = std::numeric_limits::max(); + std::optional> min_deviation; for (const auto & [i, deviations] : deviation_map) { if (deviations.empty()) { continue; } const double sum = std::accumulate(deviations.begin(), deviations.end(), 0.0); - if (sum < min_sum_deviation) { - min_sum_deviation = sum; - min_deviation_index = i; + const double mean = sum / deviations.size(); + if (!min_deviation.has_value() || mean < min_deviation.value().second) { + min_deviation = std::make_pair(i, mean); } } - deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index); - // debug: save the delayed target object and the corresponding predicted path + if (!min_deviation.has_value()) { + continue; + } + + // Save the delayed target object and the corresponding predicted path for debugging + const auto [min_deviation_index, min_mean_deviation] = min_deviation.value(); + deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index); const auto path_id = uuid + "_" + std::to_string(min_deviation_index); const auto target_stamp_object = getObjectByStamp(uuid, stamp); if (target_stamp_object.has_value()) { @@ -343,17 +358,26 @@ Stat MetricsCalculator::calcPredictedPathDeviationMetrics( } } - // Store the deviation as a metric - Stat stat; - for (const auto & [uuid, deviations] : deviation_map_for_objects) { - if (deviations.empty()) { - continue; - } - for (const auto & deviation : deviations) { - stat.add(deviation); + // Step 3: Calculate the mean and variance of the deviations for each object's selected predicted + // path. Store the results in the PredictedPathDeviationMetrics structure. + PredictedPathDeviationMetrics metrics; + for (const auto & [object_id, object_path_deviations] : deviation_map_for_objects) { + if (!object_path_deviations.empty()) { + const double sum_of_deviations = + std::accumulate(object_path_deviations.begin(), object_path_deviations.end(), 0.0); + const double mean_deviation = sum_of_deviations / object_path_deviations.size(); + metrics.mean.add(mean_deviation); + double sum_of_squared_deviations = 0.0; + for (const auto path_point_deviation : object_path_deviations) { + sum_of_squared_deviations += std::pow(path_point_deviation - mean_deviation, 2); + } + const double variance_deviation = sum_of_squared_deviations / object_path_deviations.size(); + + metrics.variance.add(variance_deviation); } } - return stat; + + return metrics; } MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const @@ -381,12 +405,17 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas 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; 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 f3a08bf42797a..ceb304894ad8c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -214,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 a29e1a468e983..523e11883e755 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -119,7 +119,7 @@ MarkerArray createDeviationLines( for (size_t i = 0; i < max_idx; ++i) { auto marker = createDefaultMarker( "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)); + createMarkerScale(0.01, 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); 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 dabd17b9db46a..e64411a66f521 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 @@ -77,11 +77,6 @@ class EvalTest : public ::testing::Test objects_pub_ = rclcpp::create_publisher( dummy_node, "/perception_online_evaluator/input/objects", 1); - marker_sub_ = rclcpp::create_subscription( - eval_node, "perception_online_evaluator/markers", 10, - [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) { - has_received_marker_ = true; - }); uuid_ = generateUUID(); } @@ -229,12 +224,10 @@ class EvalTest : public ::testing::Test void waitForDummyNode() { - // wait for the marker to be published - publishObjects(makeStraightPredictedObjects(0)); - while (!has_received_marker_) { + // Wait until the publisher is connected to the dummy node + while (objects_pub_->get_subscription_count() == 0) { rclcpp::spin_some(dummy_node); rclcpp::sleep_for(std::chrono::milliseconds(100)); - rclcpp::spin_some(eval_node); } } @@ -248,8 +241,6 @@ class EvalTest : public ::testing::Test // Pub/Sub rclcpp::Publisher::SharedPtr objects_pub_; rclcpp::Subscription::SharedPtr metric_sub_; - rclcpp::Subscription::SharedPtr marker_sub_; - bool has_received_marker_{false}; double time_delay_ = 5.0; double time_step_ = 0.5; @@ -497,7 +488,7 @@ TEST_F(EvalTest, testYawDeviation_deviation0_PEDESTRIAN) } // ========================================================================================== -// predicted path deviation{ +// predicted path deviation TEST_F(EvalTest, testPredictedPathDeviation_deviation0) { waitForDummyNode(); @@ -585,6 +576,145 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation0_PEDESTRIAN) } // ========================================================================================== +// ========================================================================================== +// predicted path deviation variance +TEST_F(EvalTest, testPredictedPathDeviationVariance_deviation0) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_variance_CAR_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 0.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviationVariance_deviation1) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_variance_CAR_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 1.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + // deviations + // All - 11 points (num_points) + // 0.0[m] - 1 points + // 1.0[m] - 10 points + const double mean_deviation = deviation * (num_points - 1) / num_points; + const double variance = + (pow(0.0 - mean_deviation, 2) + 10 * pow(1.0 - mean_deviation, 2)) / num_points; + + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), variance, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviationVariance_deviationIncreasing) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_variance_CAR_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation_step = 0.1; + double deviation = deviation_step; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + deviation += deviation_step; + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + // deviations + // All - 11 points (num_points) + // 0.0[m] - 1 points + // 0.1[m] - 1 points + // 0.2[m] - 1 points + // : + // 0.9[m] - 1 points + // 1.0[m] - 1 points + const double mean_deviation = std::invoke([&]() { + double sum = 0.0; + for (size_t i = 0; i < num_points; ++i) { + sum += static_cast(i) * deviation_step; + } + return sum / num_points; + }); + double sum_squared_deviations = 0.0; + for (size_t i = 0; i < num_points; ++i) { + const double deviation = static_cast(i) * deviation_step; + sum_squared_deviations += pow(deviation - mean_deviation, 2); + } + const double variance = sum_squared_deviations / num_points; + + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), variance, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviationVariance_deviationOscillating) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_variance_CAR_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const std::vector deviations = {-0.1, -0.2, -0.1, 0.0, 0.1, 0.2, 0.1, 0.0, -0.1, -0.2}; + for (size_t i = 0; i < deviations.size() - 1; ++i) { + const double time = static_cast(i + 1) * time_step_; + const auto objects = makeDeviatedStraightPredictedObjects(time, deviations[i]); + publishObjects(objects); + } + + const double num_points = deviations.size() + 1; + // deviations + // All - 11 points (num_points) + // 0.0[m] - 1 points + // -0.1[m] - 1 points + // -0.2[m] - 1 points + // -0.1[m] - 1 points + // -0.0[m] - 1 points + // 0.1[m] - 1 points + // 0.2[m] - 1 points + // 0.1[m] - 1 points + // 0.0[m] - 1 points + // -0.1[m] - 1 points + // -0.2[m] - 1 points + const double mean_deviation = + std::accumulate( + deviations.begin(), deviations.end(), 0.0, + [](double sum, double deviation) { return sum + std::abs(deviation); }) / + num_points; + + double sum_squared_deviations = pow(0 - mean_deviation, 2); + for (const auto deviation : deviations) { + sum_squared_deviations += pow(std::abs(deviation) - mean_deviation, 2); + } + const double variance = sum_squared_deviations / num_points; + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviations.back()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), variance, epsilon); +} +// ========================================================================================== + // ========================================================================================== // yaw rate TEST_F(EvalTest, testYawRate_0) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index f9b65fd5c30eb..ab89da7112b66 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -5,7 +5,7 @@ - + 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_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index c6213ee313c2b..d9e18e76bc4c4 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -36,7 +36,7 @@ - + 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/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index 519afa4145bcf..0f870d37c0de1 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -137,20 +137,33 @@ void DistanceBasedCompareMapFilterComponent::filter( std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - if (distance_based_map_loader_->is_close_to_map(pcl_input->points.at(i), distance_threshold_)) { + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); + if (distance_based_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(pcl_input->points.at(i)); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 34a0f17bdb389..7ad479a6e74bd 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -106,19 +106,33 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - if (voxel_based_approximate_map_loader_->is_close_to_map( - pcl_input->points.at(i), distance_threshold_)) { + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); + if (voxel_based_approximate_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(pcl_input->points.at(i)); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 82c3576dd951c..595336145dabd 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -67,19 +67,33 @@ void VoxelBasedCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - const pcl::PointXYZ point = pcl_input->points.at(i); + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); if (voxel_grid_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(point); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 32e5367fbcc38..f169e2069aee7 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -135,20 +135,34 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(*input, *pcl_input); - pcl_output->points.reserve(pcl_input->points.size()); - for (size_t i = 0; i < pcl_input->points.size(); ++i) { - if (voxel_distance_based_map_loader_->is_close_to_map( - pcl_input->points.at(i), distance_threshold_)) { + int point_step = input->point_step; + int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset; + + output.data.resize(input->data.size()); + output.point_step = point_step; + size_t output_size = 0; + for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) { + pcl::PointXYZ point{}; + std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float)); + std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float)); + std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float)); + if (voxel_distance_based_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - pcl_output->points.push_back(pcl_input->points.at(i)); + std::memcpy(&output.data[output_size], &input->data[global_offset], point_step); + output_size += point_step; } - pcl::toROSMsg(*pcl_output, output); output.header = input->header; + output.fields = input->fields; + output.data.resize(output_size); + output.height = input->height; + output.width = output_size / point_step / output.height; + output.row_step = output_size / output.height; + output.is_bigendian = input->is_bigendian; + output.is_dense = input->is_dense; // add processing time for debug if (debug_publisher_) { 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/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 3abaffb243d96..53ac6f4cafc28 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -16,6 +16,7 @@ downsample_factor: 1 encoder_in_feature_size: 12 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + has_variance: false has_twist: false densification_params: world_frame_id: "map" diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index 78ae152141a00..c39dd7ac8bc33 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -60,6 +60,7 @@ class PointPaintingFusionNode std::map class_index_; std::map> isClassTable_; std::vector pointcloud_range; + bool has_variance_{false}; bool has_twist_{false}; centerpoint::NonMaximumSuppression iou_bev_nms_; diff --git a/perception/image_projection_based_fusion/schema/pointpainting.schema.json b/perception/image_projection_based_fusion/schema/pointpainting.schema.json index 036628d72e70a..bc8f0512263b6 100644 --- a/perception/image_projection_based_fusion/schema/pointpainting.schema.json +++ b/perception/image_projection_based_fusion/schema/pointpainting.schema.json @@ -60,6 +60,11 @@ "minimum": 0.0, "maximum": 1.0 }, + "has_variance": { + "type": "boolean", + "description": "Indicates whether the model outputs variance value.", + "default": false + }, "has_twist": { "type": "boolean", "description": "Indicates whether the model outputs twist value.", diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 14783a46ba8b4..c4c5941be5fec 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -140,6 +140,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_.erase(cls); } } + has_variance_ = this->declare_parameter("has_variance"); has_twist_ = this->declare_parameter("model_params.has_twist"); const std::size_t point_feature_size = static_cast( this->declare_parameter("model_params.point_feature_size")); @@ -189,7 +190,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt centerpoint::CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, pointcloud_range, voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_thresholds); + yaw_norm_thresholds, has_variance_); // create detector detector_ptr_ = std::make_unique( @@ -401,7 +402,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, has_variance_, obj); raw_objects.emplace_back(obj); } diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index a9c3174846d0d..38c57285e552d 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -14,6 +14,7 @@ iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] score_threshold: 0.35 + has_variance: false has_twist: false trt_precision: fp16 densification_num_past_frames: 1 diff --git a/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml b/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml new file mode 100644 index 0000000000000..c217f6321381a --- /dev/null +++ b/perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 4 + max_voxel_size: 40000 + point_cloud_range: [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0] + voxel_size: [0.32, 0.32, 10.0] + downsample_factor: 1 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + score_threshold: 0.35 + has_variance: true + has_twist: true + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 474d0e2b695ac..35e11e61e9634 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -14,6 +14,7 @@ iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] score_threshold: 0.35 + has_variance: false has_twist: false trt_precision: fp16 densification_num_past_frames: 1 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 363184ecacfa9..19fdbe7a8b9c2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -28,7 +28,7 @@ class CenterPointConfig const std::vector & point_cloud_range, const std::vector & voxel_size, const std::size_t downsample_factor, const std::size_t encoder_in_feature_size, const float score_threshold, const float circle_nms_dist_threshold, - const std::vector yaw_norm_thresholds) + const std::vector yaw_norm_thresholds, const bool has_variance) { class_size_ = class_size; point_feature_size_ = point_feature_size; @@ -49,6 +49,15 @@ class CenterPointConfig downsample_factor_ = downsample_factor; encoder_in_feature_size_ = encoder_in_feature_size; + if (has_variance) { + has_variance_ = true; + head_out_offset_size_ = 4; + head_out_z_size_ = 2; + head_out_dim_size_ = 6; + head_out_rot_size_ = 4; + head_out_vel_size_ = 4; + } + if (score_threshold > 0 && score_threshold < 1) { score_threshold_ = score_threshold; } @@ -97,11 +106,12 @@ class CenterPointConfig std::size_t encoder_in_feature_size_{9}; const std::size_t encoder_out_feature_size_{32}; const std::size_t head_out_size_{6}; - const std::size_t head_out_offset_size_{2}; - const std::size_t head_out_z_size_{1}; - const std::size_t head_out_dim_size_{3}; - const std::size_t head_out_rot_size_{2}; - const std::size_t head_out_vel_size_{2}; + bool has_variance_{false}; + std::size_t head_out_offset_size_{2}; + std::size_t head_out_z_size_{1}; + std::size_t head_out_dim_size_{3}; + std::size_t head_out_rot_size_{2}; + std::size_t head_out_vel_size_{2}; // post-process params float score_threshold_{0.35f}; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 963579e5763c2..d9740df515db2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -53,6 +53,7 @@ class LidarCenterPointNode : public rclcpp::Node float score_threshold_{0.0}; std::vector class_names_; + bool has_variance_{false}; bool has_twist_{false}; NonMaximumSuppression iou_bev_nms_; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 69b75b614498e..4eb4318c4f3f9 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -32,10 +32,14 @@ namespace centerpoint void box3DToDetectedObject( const Box3D & box3d, const std::vector & class_names, const bool has_twist, - autoware_auto_perception_msgs::msg::DetectedObject & obj); + const bool has_variance, autoware_auto_perception_msgs::msg::DetectedObject & obj); uint8_t getSemanticType(const std::string & class_name); +std::array convertPoseCovarianceMatrix(const Box3D & box3d); + +std::array convertTwistCovarianceMatrix(const Box3D & box3d); + bool isCarLikeVehicleLabel(const uint8_t label); } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp index 84462bc9657ac..f3dd30f754989 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp @@ -33,6 +33,17 @@ struct Box3D float yaw; float vel_x; float vel_y; + + // variance + float x_variance; + float y_variance; + float z_variance; + float length_variance; + float width_variance; + float height_variance; + float yaw_variance; + float vel_x_variance; + float vel_y_variance; }; // cspell: ignore divup diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index ea2111862759a..34bbd2811041c 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -58,7 +58,7 @@ __global__ void generateBoxes3D_kernel( const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - const float * yaw_norm_thresholds, Box3D * det_boxes3d) + const bool has_variance, const float * yaw_norm_thresholds, Box3D * det_boxes3d) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -107,6 +107,34 @@ __global__ void generateBoxes3D_kernel( det_boxes3d[idx].yaw = atan2f(yaw_sin, yaw_cos); det_boxes3d[idx].vel_x = vel_x; det_boxes3d[idx].vel_y = vel_y; + + if (has_variance) { + const float offset_x_variance = out_offset[down_grid_size * 2 + idx]; + const float offset_y_variance = out_offset[down_grid_size * 3 + idx]; + const float z_variance = out_z[down_grid_size * 1 + idx]; + const float w_variance = out_dim[down_grid_size * 3 + idx]; + const float l_variance = out_dim[down_grid_size * 4 + idx]; + const float h_variance = out_dim[down_grid_size * 5 + idx]; + const float yaw_sin_log_variance = out_rot[down_grid_size * 2 + idx]; + const float yaw_cos_log_variance = out_rot[down_grid_size * 3 + idx]; + const float vel_x_variance = out_vel[down_grid_size * 2 + idx]; + const float vel_y_variance = out_vel[down_grid_size * 3 + idx]; + + det_boxes3d[idx].x_variance = voxel_size_x * downsample_factor * expf(offset_x_variance); + det_boxes3d[idx].y_variance = voxel_size_x * downsample_factor * expf(offset_y_variance); + det_boxes3d[idx].z_variance = expf(z_variance); + det_boxes3d[idx].length_variance = expf(l_variance); + det_boxes3d[idx].width_variance = expf(w_variance); + det_boxes3d[idx].height_variance = expf(h_variance); + const float yaw_sin_sq = yaw_sin * yaw_sin; + const float yaw_cos_sq = yaw_cos * yaw_cos; + const float yaw_norm_sq = (yaw_sin_sq + yaw_cos_sq) * (yaw_sin_sq + yaw_cos_sq); + det_boxes3d[idx].yaw_variance = + (yaw_cos_sq * expf(yaw_sin_log_variance) + yaw_sin_sq * expf(yaw_cos_log_variance)) / + yaw_norm_sq; + det_boxes3d[idx].vel_x_variance = expf(vel_x_variance); + det_boxes3d[idx].vel_y_variance = expf(vel_y_variance); + } } PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(config) @@ -131,7 +159,7 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, - thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), + config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), thrust::raw_pointer_cast(boxes3d_d_.data())); // suppress by score diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 6bb788ec097b5..c4fb4abd8d753 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -25,7 +25,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; void box3DToDetectedObject( const Box3D & box3d, const std::vector & class_names, const bool has_twist, - autoware_auto_perception_msgs::msg::DetectedObject & obj) + const bool has_variance, autoware_auto_perception_msgs::msg::DetectedObject & obj) { // TODO(yukke42): the value of classification confidence of DNN, not probability. obj.existence_probability = box3d.score; @@ -58,6 +58,10 @@ void box3DToDetectedObject( obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; obj.shape.dimensions = tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); + if (has_variance) { + obj.kinematics.has_position_covariance = has_variance; + obj.kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(box3d); + } // twist if (has_twist) { @@ -68,6 +72,10 @@ void box3DToDetectedObject( twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; + if (has_variance) { + obj.kinematics.has_twist_covariance = has_variance; + obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); + } } } @@ -92,4 +100,24 @@ uint8_t getSemanticType(const std::string & class_name) } } +std::array convertPoseCovarianceMatrix(const Box3D & box3d) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + std::array pose_covariance{}; + pose_covariance[POSE_IDX::X_X] = box3d.x_variance; + pose_covariance[POSE_IDX::Y_Y] = box3d.y_variance; + pose_covariance[POSE_IDX::Z_Z] = box3d.z_variance; + pose_covariance[POSE_IDX::YAW_YAW] = box3d.yaw_variance; + return pose_covariance; +} + +std::array convertTwistCovarianceMatrix(const Box3D & box3d) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + std::array twist_covariance{}; + twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; + twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; + return twist_covariance; +} + } // namespace centerpoint diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 65bdafe94fe7d..0606a2ec55c9e 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -55,7 +55,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); - has_twist_ = this->declare_parameter("has_twist", false); + has_variance_ = this->declare_parameter("has_variance"); + has_twist_ = this->declare_parameter("has_twist"); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); const std::size_t max_voxel_size = @@ -102,7 +103,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_thresholds); + yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); @@ -152,7 +153,7 @@ void LidarCenterPointNode::pointCloudCallback( raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, has_variance_, obj); raw_objects.emplace_back(obj); } 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 8d835fec20a9b..41d150ef0ba1b 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -23,6 +23,7 @@ include_directories( 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 @@ -47,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 index ca1b20d26c25d..90291ae6fec18 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp @@ -41,7 +41,10 @@ class TrackerDebugger 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 @@ -53,15 +56,19 @@ class TrackerDebugger } 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_; }; 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 94cc7185bd518..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 @@ -21,6 +21,7 @@ #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 @@ -46,6 +47,7 @@ #include #include #include +#include #include class MultiObjectTracker : public rclcpp::Node @@ -54,39 +56,35 @@ class MultiObjectTracker : public rclcpp::Node 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 index e3fc58dd5d692..b5632a13e78fc 100644 --- a/perception/multi_object_tracker/src/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger.cpp @@ -18,12 +18,7 @@ #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) -: diagnostic_updater_(&node), - node_(node), - last_input_stamp_(node.now()), - stamp_process_start_(node.now()), - stamp_publish_output_(node.now()) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -39,7 +34,15 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) "debug/tentative_objects", rclcpp::QoS{1}); } - // initialize stop watch and diagnostics + // 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(); } @@ -73,7 +76,11 @@ void TrackerDebugger::loadParameters() void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) { - const double delay = pipeline_latency_ms_; // [s] + 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."); @@ -108,18 +115,40 @@ void TrackerDebugger::startMeasurementTime( 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) { - const auto current_time = now; - // pipeline latency: time from sensor measurement to publish - pipeline_latency_ms_ = (current_time - last_input_stamp_).seconds() * 1e3; + // 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 time: time between input and publish - double processing_time_ms = (current_time - stamp_process_start_).seconds() * 1e3; + // 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 = (current_time - stamp_publish_output_).seconds() * 1e3; + 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; @@ -131,8 +160,10 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim "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_ = current_time; + 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 b8e19d4ca2df4..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, @@ -69,12 +70,9 @@ boost::optional getTransformAnonymous( 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}, @@ -82,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)); + } + + // 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); } - 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); + // 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(this->now(), 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) { @@ -345,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 + // Publish debugger information if enabled debugger_->endPublishTime(this->now(), time); - debugger_->publishTentativeObjects(tentative_objects_msg); + + 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 874d542b445d3..62f35a053958a 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -551,13 +551,15 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi #### Additional parameters -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `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 | -| `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 | +| 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 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/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/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 523a5349aaef6..af2ccb807d989 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -136,8 +136,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = getPreviousModuleOutput().path; - BehaviorModuleOutput out; - out = module_type_->getTerminalLaneChangePath(); + BehaviorModuleOutput out = getPreviousModuleOutput(); module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); out.turn_signal_info = getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); 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 08034d43feb43..db96f7c83d72a 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -889,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); } @@ -969,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); @@ -1289,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); @@ -1343,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); @@ -1964,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( @@ -2059,6 +2062,17 @@ 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()) { @@ -2113,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/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/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 0126ab47d1ae6..3089134a16500 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -51,7 +51,7 @@ class PullOutPlannerBase } virtual ~PullOutPlannerBase() = default; - void setPlannerData(std::shared_ptr & planner_data) + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 2d661cf41855f..c05b7a8f94716 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -155,6 +155,24 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + struct StartPlannerData + { + StartPlannerParameters parameters; + PlannerData planner_data; + ModuleStatus current_status; + PullOutStatus main_thread_pull_out_status; + bool is_stopped; + + StartPlannerData clone() const; + void update( + const StartPlannerParameters & parameters, const PlannerData & planner_data, + const ModuleStatus & current_status, const PullOutStatus & pull_out_status, + const bool is_stopped); + }; + std::optional freespace_thread_status_{std::nullopt}; + std::optional start_planner_data_{std::nullopt}; + std::mutex start_planner_data_mutex_; + // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag { @@ -290,7 +308,6 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; bool isStopped(); - bool isStuck(); bool hasFinishedCurrentPath(); void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, @@ -307,7 +324,9 @@ class StartPlannerModule : public SceneModuleInterface SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); - bool planFreespacePath(); + std::optional planFreespacePath( + const StartPlannerParameters & parameters, + const std::shared_ptr & planner_data, const PullOutStatus & pull_out_status); void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 0e6e5fae84fb8..4433020605056 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -93,26 +93,56 @@ void StartPlannerModule::onFreespacePlannerTimer() { const ScopedFlag flag(is_freespace_planner_cb_running_); - if (getCurrentStatus() == ModuleStatus::IDLE) { + std::shared_ptr local_planner_data{nullptr}; + std::optional current_status_opt{std::nullopt}; + std::optional parameters_opt{std::nullopt}; + std::optional pull_out_status_opt{std::nullopt}; + bool is_stopped; + + // making a local copy of thread sensitive data + { + std::lock_guard guard(start_planner_data_mutex_); + if (start_planner_data_) { + const auto & start_planner_data = start_planner_data_.value(); + local_planner_data = std::make_shared(start_planner_data.planner_data); + current_status_opt = start_planner_data.current_status; + parameters_opt = start_planner_data.parameters; + pull_out_status_opt = start_planner_data.main_thread_pull_out_status; + is_stopped = start_planner_data.is_stopped; + } + } + // finish copying thread sensitive data + if (!local_planner_data || !current_status_opt || !parameters_opt || !pull_out_status_opt) { return; } - if (!planner_data_) { + const auto & current_status = current_status_opt.value(); + const auto & parameters = parameters_opt.value(); + const auto & pull_out_status = pull_out_status_opt.value(); + + if (current_status == ModuleStatus::IDLE) { return; } - if (!planner_data_->costmap) { + if (!local_planner_data->costmap) { return; } const bool is_new_costmap = - (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; if (!is_new_costmap) { return; } - if (isStuck()) { - planFreespacePath(); + const bool is_stuck = is_stopped && pull_out_status.planner_type == PlannerType::STOP && + !pull_out_status.found_pull_out_path; + if (is_stuck) { + const auto free_space_status = + planFreespacePath(parameters, local_planner_data, pull_out_status); + if (free_space_status) { + std::lock_guard guard(start_planner_data_mutex_); + freespace_thread_status_ = free_space_status; + } } } @@ -172,6 +202,38 @@ void StartPlannerModule::updateObjectsFilteringParams( void StartPlannerModule::updateData() { + // The method PlannerManager::run() calls SceneModuleInterface::setData and + // SceneModuleInterface::setPreviousModuleOutput() before this module's run() method is called + // with module_ptr->run(). Then module_ptr->run() invokes StartPlannerModule::updateData and, + // finally, the planWaitingApproval()/plan() methods are called by run(). So we can copy the + // latest current_status to start_planner_data_ here for later usage. + + // NOTE: onFreespacePlannerTimer copies start_planner_data to its thread local variable, so we + // need to lock start_planner_data_ here to avoid data race. But the following clone process is + // lightweight because most of the member variables of PlannerData/RouteHandler is + // shared_ptrs/bool + // making a local copy of thread sensitive data + { + std::lock_guard guard(start_planner_data_mutex_); + if (!start_planner_data_) { + start_planner_data_ = StartPlannerData(); + } + start_planner_data_.value().update( + *parameters_, *planner_data_, getCurrentStatus(), status_, isStopped()); + if (freespace_thread_status_) { + // if freespace solution is available, copy it to status_ on main thread + const auto & freespace_status = freespace_thread_status_.value(); + status_.pull_out_path = freespace_status.pull_out_path; + status_.pull_out_start_pose = freespace_status.pull_out_start_pose; + status_.planner_type = freespace_status.planner_type; + status_.found_pull_out_path = freespace_status.found_pull_out_path; + status_.driving_forward = freespace_status.driving_forward; + // and then reset it + freespace_thread_status_ = std::nullopt; + } + } + // finish copying thread sensitive data + if (receivedNewRoute()) { resetStatus(); DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); @@ -1128,19 +1190,6 @@ bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const return elapsed < parameters_->prepare_time_before_start; } -bool StartPlannerModule::isStuck() -{ - if (!isStopped()) { - return false; - } - - if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) { - return true; - } - - return false; -} - bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -1336,19 +1385,21 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() return output; } -bool StartPlannerModule::planFreespacePath() +std::optional StartPlannerModule::planFreespacePath( + const StartPlannerParameters & parameters, + const std::shared_ptr & planner_data, const PullOutStatus & pull_out_status) { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto & route_handler = planner_data_->route_handler; + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; - const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; - const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; - const double end_pose_search_interval = parameters_->end_pose_search_interval; + const double end_pose_search_start_distance = parameters.end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters.end_pose_search_end_distance; + const double end_pose_search_interval = parameters.end_pose_search_interval; const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + planner_data->parameters.backward_path_length + parameters.max_back_distance; const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); @@ -1361,23 +1412,23 @@ bool StartPlannerModule::planFreespacePath() for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - freespace_planner_->setPlannerData(planner_data_); + freespace_planner_->setPlannerData(planner_data); auto freespace_path = freespace_planner_->plan(current_pose, end_pose); if (!freespace_path) { continue; } - const std::lock_guard lock(mutex_); - status_.pull_out_path = *freespace_path; - status_.pull_out_start_pose = current_pose; - status_.planner_type = freespace_planner_->getPlannerType(); - status_.found_pull_out_path = true; - status_.driving_forward = true; - return true; + auto status = pull_out_status; + status.pull_out_path = *freespace_path; + status.pull_out_start_pose = current_pose; + status.planner_type = freespace_planner_->getPlannerType(); + status.found_pull_out_path = true; + status.driving_forward = true; + return std::make_optional(status); } - return false; + return std::nullopt; } void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -1665,4 +1716,43 @@ void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const logFunc("======================================="); } + +StartPlannerModule::StartPlannerData StartPlannerModule::StartPlannerData::clone() const +{ + StartPlannerData start_planner_data; + start_planner_data.update( + parameters, planner_data, current_status, main_thread_pull_out_status, is_stopped); + return start_planner_data; +} + +void StartPlannerModule::StartPlannerData::update( + const StartPlannerParameters & parameters_, const PlannerData & planner_data_, + const ModuleStatus & current_status_, const PullOutStatus & pull_out_status_, + const bool is_stopped_) +{ + parameters = parameters_; + planner_data = planner_data_; + // TODO(Mamoru Sobue): in the future we will add planner_data->is_route_handler_updated flag to + // avoid the copy overhead of lanelet objects inside the RouteHandler. behavior_path_planner can + // tell us the flag if map/route changed, so we can skip route_handler update if it + // is false in the following way + /* + auto route_handler_self = planner_data.route_handler; + planner_data = planner_data_; // sync planer_data to planner_data_, planner_data.route_handler + is once re-pointed + + if (!planner_data_->is_route_handler_updated && route_handler_self != nullptr) { + // we do not need to sync planner_data.route_handler with that of planner_data_ + // re-point to the original again + planner_data.route_handler = route_handler_self; + } else { + // this is actually heavy if the lanelet_map is HUGE + planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); + } + */ + planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); + current_status = current_status_; + main_thread_pull_out_status = pull_out_status_; + is_stopped = is_stopped_; +} } // namespace behavior_path_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 6c9254919cac7..b26f960ec28f9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -296,7 +296,10 @@ bool IntersectionModule::checkStuckVehicleInIntersection( // check if the footprint is in the stuck detect area const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + // NOTE: in order not to stop too much + const bool is_in_stuck_area = bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + stuck_vehicle_detect_area); if (is_in_stuck_area) { debug_data_.stuck_targets.objects.push_back(object); return true; 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 628b8725149a2..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 @@ -17,6 +17,7 @@ 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/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index e2ce3822bf86d..1c85a22f57bf6 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -77,6 +77,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) 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 48fbebb8b056d..da826e1cf0cf6 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -144,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, @@ -152,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 @@ -230,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 {}; @@ -717,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 @@ -736,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( @@ -806,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( diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index c7702ed15337d..3db6051ab36e7 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -66,6 +66,9 @@ 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_; @@ -119,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); @@ -128,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); diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 823dc81b2b72b..51b460482458f 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -68,6 +68,7 @@ struct RunOutParam 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; 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/docs/mpt.md b/planning/obstacle_avoidance_planner/docs/mpt.md index f8441783bf1d2..4b35fd0e36ab5 100644 --- a/planning/obstacle_avoidance_planner/docs/mpt.md +++ b/planning/obstacle_avoidance_planner/docs/mpt.md @@ -51,8 +51,8 @@ We also assume that the steer angle $\delta_k$ is first-order lag to the command $$ \begin{align} -y_{k+1} & = y_{k} + v \sin \theta_k dt \\ -\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\ +y_{k+1} & = y_{k} + v \sin \theta_k dt \\\ +\theta_{k+1} & = \theta_k + \frac{v \tan \delta_k}{L}dt - \kappa_k v \cos \theta_k dt \\\ \delta_{k+1} & = \delta_k - \frac{\delta_k - \delta_{des,k}}{\tau}dt \end{align} $$ @@ -72,8 +72,8 @@ Therefore, we have to apply the steer angle limitation to $\delta_{\mathrm{ref}, $$ \begin{align} -\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\ -\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\ +\delta_{\mathrm{ref}, k} & = \mathrm{clamp}(\arctan(L \kappa_k), -\delta_{\max}, \delta_{\max}) \\\ +\delta_k & = \delta_{\mathrm{ref}, k} + \Delta \delta_k, \ \Delta \delta_k \ll 1 \\\ \end{align} $$ @@ -83,8 +83,8 @@ Using this $\delta_{\mathrm{ref}, k}$, $\tan \delta_k$ is linearized as follows. $$ \begin{align} -\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\ -& = \tan \delta_{\mathrm{ref}, k} + \left.\frac{d \tan \delta}{d \delta}\right|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\ +\tan \delta_k & \approx \tan \delta_{\mathrm{ref}, k} + \lbrace\frac{d \tan \delta}{d \delta}\rbrace|_{\delta = \delta_{\mathrm{ref}, k}} \Delta \delta_k \\\ +& = \tan \delta_{\mathrm{ref}, k} + \lbrace\frac{d \tan \delta}{d \delta}\rbrace|_{\delta = \delta_{\mathrm{ref}, k}} (\delta_{\mathrm{ref}, k} - \delta_k) \\\ & = \tan \delta_{\mathrm{ref}, k} - \frac{\delta_{\mathrm{ref}, k}}{\cos^2 \delta_{\mathrm{ref}, k}} + \frac{1}{\cos^2 \delta_{\mathrm{ref}, k}} \delta_k \end{align} $$ @@ -96,37 +96,37 @@ Based on the linearization, the error kinematics is formulated with the followin $$ \begin{align} \begin{pmatrix} - y_{k+1} \\ + y_{k+1} \\\ \theta_{k+1} \end{pmatrix} = \begin{pmatrix} - 1 & v dt \\ - 0 & 1 \\ + 1 & v dt \\\ + 0 & 1 \end{pmatrix} \begin{pmatrix} - y_k \\ - \theta_k \\ + y_k \\\ + \theta_k \end{pmatrix} + \begin{pmatrix} - 0 \\ - \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \\ + 0 \\\ + \frac{v dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} \end{pmatrix} \delta_{k} + \begin{pmatrix} - 0 \\ - \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt\\ + 0 \\\ + \frac{v \tan(\delta_{\mathrm{ref}, k}) dt}{L} - \frac{v \delta_{\mathrm{ref}, k} dt}{L \cos^{2} \delta_{\mathrm{ref}, k}} - \kappa_k v dt \end{pmatrix} \end{align} $$ -which can be formulated as follows with the state $\boldsymbol{x}$, control input $u$ and some matrices, where $\boldsymbol{x} = (y_k, \theta_k)$ +which can be formulated as follows with the state $\mathbf{x}$, control input $u$ and some matrices, where $\mathbf{x} = (y_k, \theta_k)$ $$ \begin{align} - \boldsymbol{x}_{k+1} = A_k \boldsymbol{x}_k + \boldsymbol{b}_k u_k + \boldsymbol{w}_k + \mathbf{x}_{k+1} = A_k \mathbf{x}_k + \mathbf{b}_k u_k + \mathbf{w}_k \end{align} $$ @@ -136,7 +136,7 @@ Then, we formulate time-series state equation by concatenating states, control i $$ \begin{align} - \boldsymbol{x} = A \boldsymbol{x}_0 + B \boldsymbol{u} + \boldsymbol{w} + \mathbf{x} = A \mathbf{x}_0 + B \mathbf{u} + \mathbf{w} \end{align} $$ @@ -144,9 +144,9 @@ where $$ \begin{align} -\boldsymbol{x} = (\boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \boldsymbol{x}^T_3, \dots, \boldsymbol{x}^T_{n-1})^T \\ -\boldsymbol{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\ -\boldsymbol{w} = (\boldsymbol{w}^T_0, \boldsymbol{w}^T_1, \boldsymbol{w}^T_2, \dots, \boldsymbol{w}^T_{n-1})^T. \\ +\mathbf{x} = (\mathbf{x}^T_1, \mathbf{x}^T_2, \mathbf{x}^T_3, \dots, \mathbf{x}^T_{n-1})^T \\\ +\mathbf{u} = (u_0, u_1, u_2, \dots, u_{n-2})^T \\\ +\mathbf{w} = (\mathbf{w}^T_0, \mathbf{w}^T_1, \mathbf{w}^T_2, \dots, \mathbf{w}^T_{n-1})^T. \\\ \end{align} $$ @@ -155,63 +155,63 @@ In detail, each matrices are constructed as follows. $$ \begin{align} \begin{pmatrix} - \boldsymbol{x}_1 \\ - \boldsymbol{x}_2 \\ - \boldsymbol{x}_3 \\ - \vdots \\ - \boldsymbol{x}_{n-1} + \mathbf{x}_1 \\\ + \mathbf{x}_2 \\\ + \mathbf{x}_3 \\\ + \vdots \\\ + \mathbf{x}_{n-1} \end{pmatrix} = \begin{pmatrix} - A_0 \\ - A_1 A_0 \\ - A_2 A_1 A_0\\ - \vdots \\ + A_0 \\\ + A_1 A_0 \\\ + A_2 A_1 A_0\\\ + \vdots \\\ \prod\limits_{k=0}^{n-1} A_{k} \end{pmatrix} - \boldsymbol{x}_0 + \mathbf{x}_0 + \begin{pmatrix} - B_0 & 0 & & \dots & 0 \\ - A_0 B_0 & B_1 & 0 & \dots & 0 \\ - A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ + B_0 & 0 & & \dots & 0 \\\ + A_0 B_0 & B_1 & 0 & \dots & 0 \\\ + A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\\ + \vdots & \vdots & & \ddots & 0 \\\ \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} \end{pmatrix} \begin{pmatrix} - u_0 \\ - u_1 \\ - u_2 \\ - \vdots \\ + u_0 \\\ + u_1 \\\ + u_2 \\\ + \vdots \\\ u_{n-2} \end{pmatrix} + \begin{pmatrix} - I & 0 & & \dots & 0 \\ - A_0 & I & 0 & \dots & 0 \\ - A_1 A_0 & A_0 & I & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ + I & 0 & & \dots & 0 \\\ + A_0 & I & 0 & \dots & 0 \\\ + A_1 A_0 & A_0 & I & \dots & 0 \\\ + \vdots & \vdots & & \ddots & 0 \\\ \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I \end{pmatrix} \begin{pmatrix} - \boldsymbol{w}_0 \\ - \boldsymbol{w}_1 \\ - \boldsymbol{w}_2 \\ - \vdots \\ - \boldsymbol{w}_{n-2} + \mathbf{w}_0 \\\ + \mathbf{w}_1 \\\ + \mathbf{w}_2 \\\ + \vdots \\\ + \mathbf{w}_{n-2} \end{pmatrix} \end{align} $$ ### Free-boundary-conditioned time-series state equation -For path planning which does not start from the current ego pose, $\boldsymbol{x}_0$ should be the design variable of optimization. -Therefore, we make $\boldsymbol{u}'$ by concatenating $\boldsymbol{x}_0$ and $\boldsymbol{u}$, and redefine $\boldsymbol{x}$ as follows. +For path planning which does not start from the current ego pose, $\mathbf{x}_0$ should be the design variable of optimization. +Therefore, we make $\mathbf{u}'$ by concatenating $\mathbf{x}_0$ and $\mathbf{u}$, and redefine $\mathbf{x}$ as follows. $$ \begin{align} - \boldsymbol{u}' & = (\boldsymbol{x}^T_0, \boldsymbol{u}^T)^T \\ - \boldsymbol{x} & = (\boldsymbol{x}^T_0, \boldsymbol{x}^T_1, \boldsymbol{x}^T_2, \dots, \boldsymbol{x}^T_{n-1})^T + \mathbf{u}' & = (\mathbf{x}^T_0, \mathbf{u}^T)^T \\\ + \mathbf{x} & = (\mathbf{x}^T_0, \mathbf{x}^T_1, \mathbf{x}^T_2, \dots, \mathbf{x}^T_{n-1})^T \end{align} $$ @@ -219,7 +219,7 @@ Then we get the following state equation $$ \begin{align} - \boldsymbol{x}' = B \boldsymbol{u}' + \boldsymbol{w}, + \mathbf{x}' = B \mathbf{u}' + \mathbf{w}, \end{align} $$ @@ -228,45 +228,45 @@ which is in detail $$ \begin{align} \begin{pmatrix} - \boldsymbol{x}_0 \\ - \boldsymbol{x}_1 \\ - \boldsymbol{x}_2 \\ - \boldsymbol{x}_3 \\ - \vdots \\ - \boldsymbol{x}_{n-1} + \mathbf{x}_0 \\\ + \mathbf{x}_1 \\\ + \mathbf{x}_2 \\\ + \mathbf{x}_3 \\\ + \vdots \\\ + \mathbf{x}_{n-1} \end{pmatrix} = \begin{pmatrix} - I & 0 & \dots & & & 0 \\ - A_0 & B_0 & 0 & & \dots & 0 \\ - A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\ - A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\ - \vdots & \vdots & \vdots & & \ddots & 0 \\ + I & 0 & \dots & & & 0 \\\ + A_0 & B_0 & 0 & & \dots & 0 \\\ + A_1 A_0 & A_0 B_0 & B_1 & 0 & \dots & 0 \\\ + A_2 A_1 A_0 & A_1 A_0 B_0 & A_0 B_1 & B_2 & \dots & 0 \\\ + \vdots & \vdots & \vdots & & \ddots & 0 \\\ \prod\limits_{k=0}^{n-1} A_k & \prod\limits_{k=0}^{n-3} A_k B_0 & \prod\limits_{k=0}^{n-4} A_k B_1 & \dots & A_0 B_{n-3} & B_{n-2} \end{pmatrix} \begin{pmatrix} - \boldsymbol{x}_0 \\ - u_0 \\ - u_1 \\ - u_2 \\ - \vdots \\ + \mathbf{x}_0 \\\ + u_0 \\\ + u_1 \\\ + u_2 \\\ + \vdots \\\ u_{n-2} \end{pmatrix} + \begin{pmatrix} - 0 & \dots & & & 0 \\ - I & 0 & & \dots & 0 \\ - A_0 & I & 0 & \dots & 0 \\ - A_1 A_0 & A_0 & I & \dots & 0 \\ - \vdots & \vdots & & \ddots & 0 \\ + 0 & \dots & & & 0 \\\ + I & 0 & & \dots & 0 \\\ + A_0 & I & 0 & \dots & 0 \\\ + A_1 A_0 & A_0 & I & \dots & 0 \\\ + \vdots & \vdots & & \ddots & 0 \\\ \prod\limits_{k=0}^{n-3} A_k & \prod\limits_{k=0}^{n-4} A_k & \dots & A_0 & I \end{pmatrix} \begin{pmatrix} - \boldsymbol{w}_0 \\ - \boldsymbol{w}_1 \\ - \boldsymbol{w}_2 \\ - \vdots \\ - \boldsymbol{w}_{n-2} + \mathbf{w}_0 \\\ + \mathbf{w}_1 \\\ + \mathbf{w}_2 \\\ + \vdots \\\ + \mathbf{w}_{n-2} \end{pmatrix}. \end{align} $$ @@ -277,9 +277,9 @@ The objective function for smoothing and tracking is shown as follows, which can $$ \begin{align} -J_1 (\boldsymbol{x}', \boldsymbol{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot{\delta}} \sum_k \dot{\delta}_k^2 + w_{\ddot{\delta}} \sum_k \ddot{\delta}_k^2 \\ -& = \boldsymbol{x}'^T Q \boldsymbol{x}' + \boldsymbol{u}'^T R \boldsymbol{u}' \\ -& = \boldsymbol{u}'^T H \boldsymbol{u}' + \boldsymbol{u}'^T \boldsymbol{f} +J_1 (\mathbf{x}', \mathbf{u}') & = w_y \sum_{k} y_k^2 + w_{\theta} \sum_{k} \theta_k^2 + w_{\delta} \sum_k \delta_k^2 + w_{\dot \delta} \sum_k \dot \delta_k^2 + w_{\ddot \delta} \sum_k \ddot \delta_k^2 \\\ +& = \mathbf{x}'^T Q \mathbf{x}' + \mathbf{u}'^T R \mathbf{u}' \\\ +& = \mathbf{u}'^T H \mathbf{u}' + \mathbf{u}'^T \mathbf{f} \end{align} $$ @@ -287,11 +287,11 @@ As mentioned before, the constraints to be collision free with obstacles and roa Assuming that the lateral distance to the road boundaries or obstacles from the back wheel center, front wheel center, and the point between them are $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ respectively, and slack variables for each point are $\lambda_{\mathrm{base}}, \lambda_{\mathrm{top}}, \lambda_{\mathrm{mid}}$, the soft constraints can be formulated as follows. $$ -y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\ -y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\ -y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\ -0 \leq \lambda_{\mathrm{base}, k} \\ -0 \leq \lambda_{\mathrm{top}, k} \\ +y_{\mathrm{base}, k, \min} - \lambda_{\mathrm{base}, k} \leq y_{\mathrm{base}, k} (y_k) \leq y_{\mathrm{base}, k, \max} + \lambda_{\mathrm{base}, k}\\\ +y_{\mathrm{top}, k, \min} - \lambda_{\mathrm{top}, k} \leq y_{\mathrm{top}, k} (y_k) \leq y_{\mathrm{top}, k, \max} + \lambda_{\mathrm{top}, k}\\\ +y_{\mathrm{mid}, k, \min} - \lambda_{\mathrm{mid}, k} \leq y_{\mathrm{mid}, k} (y_k) \leq y_{\mathrm{mid}, k, \max} + \lambda_{\mathrm{mid}, k} \\\ +0 \leq \lambda_{\mathrm{base}, k} \\\ +0 \leq \lambda_{\mathrm{top}, k} \\\ 0 \leq \lambda_{\mathrm{mid}, k} $$ @@ -299,19 +299,19 @@ Since $y_{\mathrm{base}, k}, y_{\mathrm{top}, k}, y_{\mathrm{mid}, k}$ is formul $$ \begin{align} -J_2 & (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol {\lambda}_\mathrm{mid})\\ +J_2 & (\mathbf{\lambda}_\mathrm{base}, \mathbf{\lambda}_\mathrm{top}, \mathbf {\lambda}_\mathrm{mid})\\\ & = w_{\mathrm{base}} \sum_{k} \lambda_{\mathrm{base}, k} + w_{\mathrm{mid}} \sum_k \lambda_{\mathrm{mid}, k} + w_{\mathrm{top}} \sum_k \lambda_{\mathrm{top}, k} \end{align} $$ Slack variables are also design variables for optimization. -We define a vector $\boldsymbol{v}$, that concatenates all the design variables. +We define a vector $\mathbf{v}$, that concatenates all the design variables. $$ \begin{align} -\boldsymbol{v} = +\mathbf{v} = \begin{pmatrix} - \boldsymbol{u}'^T & \boldsymbol{\lambda}_\mathrm{base}^T & \boldsymbol{\lambda}_\mathrm{top}^T & \boldsymbol{\lambda}_\mathrm{mid}^T + \mathbf{u}'^T & \mathbf{\lambda}_\mathrm{base}^T & \mathbf{\lambda}_\mathrm{top}^T & \mathbf{\lambda}_\mathrm{mid}^T \end{pmatrix}^T \end{align} $$ @@ -320,7 +320,7 @@ The summation of these two objective functions is the objective function for the $$ \begin{align} -\min_{\boldsymbol{v}} J (\boldsymbol{v}) = \min_{\boldsymbol{v}} J_1 (\boldsymbol{u}') + J_2 (\boldsymbol{\lambda}_\mathrm{base}, \boldsymbol{\lambda}_\mathrm{top}, \boldsymbol{\lambda}_\mathrm{mid}) +\min_{\mathbf{v}} J (\mathbf{v}) = \min_{\mathbf{v}} J_1 (\mathbf{u}') + J_2 (\mathbf{\lambda}_\mathrm{base}, \mathbf{\lambda}_\mathrm{top}, \mathbf{\lambda}_\mathrm{mid}) \end{align} $$ @@ -337,8 +337,8 @@ Finally we transform those objective functions to the following QP problem, and $$ \begin{align} -\min_{\boldsymbol{v}} \ & \frac{1}{2} \boldsymbol{v}^T \boldsymbol{H} \boldsymbol{v} + \boldsymbol{f} \boldsymbol{v} \\ -\mathrm{s.t.} \ & \boldsymbol{b}_{lower} \leq \boldsymbol{A} \boldsymbol{v} \leq \boldsymbol{b}_{upper} +\min_{\mathbf{v}} \ & \frac{1}{2} \mathbf{v}^T \mathbf{H} \mathbf{v} + \mathbf{f} \mathbf{v} \\\ +\mathrm{s.t.} \ & \mathbf{b}_{lower} \leq \mathbf{A} \mathbf{v} \leq \mathbf{b}_{upper} \end{align} $$ @@ -382,8 +382,8 @@ Based on the following figure, $y'$ can be formulated as follows. $$ \begin{align} -y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\ -& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\ +y' & = L \sin(\theta + \beta) + y \cos \beta - l \sin(\gamma - \phi_a) \\\ +& = L \sin \theta \cos \beta + L \cos \theta \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \\\ & \approx L \theta \cos \beta + L \sin \beta + y \cos \beta - l \sin(\gamma - \phi_a) \end{align} $$ @@ -394,9 +394,9 @@ $$ $$ \begin{align} -y' & = C_1 \boldsymbol{x} + C_2 \\ -& = C_1 (B \boldsymbol{v} + \boldsymbol{w}) + C_2 \\ -& = C_1 B \boldsymbol{v} + \boldsymbol{w} + C_2 +y' & = C_1 \mathbf{x} + C_2 \\\ +& = C_1 (B \mathbf{v} + \mathbf{w}) + C_2 \\\ +& = C_1 B \mathbf{v} + \mathbf{w} + C_2 \end{align} $$ @@ -408,20 +408,20 @@ $$ \begin{align} A_{blk} & = \begin{pmatrix} - C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\ - -C_1 B & O & \dots & O & I & O \dots & O\\ + C_1 B & O & \dots & O & I_{N_{ref} \times N_{ref}} & O \dots & O\\\ + -C_1 B & O & \dots & O & I & O \dots & O\\\ O & O & \dots & O & I & O \dots & O \end{pmatrix} - \in \boldsymbol{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\ - \boldsymbol{b}_{lower, blk} & = + \in \mathbf{R}^{3 N_{ref} \times D_v + N_{circle} N_{ref}} \\\ + \mathbf{b}_{lower, blk} & = \begin{pmatrix} - \boldsymbol{b}_{lower} - C_1 \boldsymbol{w} - C_2 \\ - -\boldsymbol{b}_{upper} + C_1 \boldsymbol{w} + C_2 \\ + \mathbf{b}_{lower} - C_1 \mathbf{w} - C_2 \\\ + -\mathbf{b}_{upper} + C_1 \mathbf{w} + C_2 \\\ O \end{pmatrix} - \in \boldsymbol{R}^{3 N_{ref}} \\ - \boldsymbol{b}_{upper, blk} & = \boldsymbol{\infty} - \in \boldsymbol{R}^{3 N_{ref}} + \in \mathbf{R}^{3 N_{ref}} \\\ + \mathbf{b}_{upper, blk} & = \mathbf{\infty} + \in \mathbf{R}^{3 N_{ref}} \end{align} $$ @@ -436,11 +436,11 @@ $$ \begin{align} A_{blk} = \begin{pmatrix} - C_1 B & I_{N_{ref} \times N_{ref}} \\ - -C_1 B & I \\ + C_1 B & I_{N_{ref} \times N_{ref}} \\\ + -C_1 B & I \\\ O & I \end{pmatrix} -\in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} +\in \mathbf{R}^{3N_{ref} \times D_v + N_{ref}} \end{align} $$ @@ -476,8 +476,8 @@ Assume that $v_{ego}$ is the ego velocity, and $t_{fix}$ is the time to fix the $$ \begin{align} -r & = \mathrm{lerp}(w^{\mathrm{steer}}_{\mathrm{normal}}, w^{\mathrm{steer}}_{\mathrm{avoidance}}, c) \\ -w^{\mathrm{lat}} & = \mathrm{lerp}(w^{\mathrm{lat}}_{\mathrm{normal}}, w^{\mathrm{lat}}_{\mathrm{avoidance}}, r) \\ +r & = \mathrm{lerp}(w^{\mathrm{steer}}_{\mathrm{normal}}, w^{\mathrm{steer}}_{\mathrm{avoidance}}, c) \\\ +w^{\mathrm{lat}} & = \mathrm{lerp}(w^{\mathrm{lat}}_{\mathrm{normal}}, w^{\mathrm{lat}}_{\mathrm{avoidance}}, r) \\\ w^{\mathrm{yaw}} & = \mathrm{lerp}(w^{\mathrm{yaw}}_{\mathrm{normal}}, w^{\mathrm{yaw}}_{\mathrm{avoidance}}, r) \end{align} $$ 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_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/docs/eb.md b/planning/path_smoother/docs/eb.md index 2c232cfa4de81..d62a6de5e3f54 100644 --- a/planning/path_smoother/docs/eb.md +++ b/planning/path_smoother/docs/eb.md @@ -78,65 +78,65 @@ We formulate a quadratic problem minimizing the diagonal length of the rhombus o ![eb](../media/eb.svg){: style="width:600px"} -Assuming that $k$'th point is $\boldsymbol{p}_k = (x_k, y_k)$, the objective function is as follows. +Assuming that $k$'th point is $\mathbf{p}_k = (x_k, y_k)$, the objective function is as follows. $$ \begin{align} -\ J & = \min \sum_{k=1}^{n-2} ||(\boldsymbol{p}_{k+1} - \boldsymbol{p}_{k}) - (\boldsymbol{p}_{k} - \boldsymbol{p}_{k-1})||^2 \\ -\ & = \min \sum_{k=1}^{n-2} ||\boldsymbol{p}_{k+1} - 2 \boldsymbol{p}_{k} + \boldsymbol{p}_{k-1}||^2 \\ -\ & = \min \sum_{k=1}^{n-2} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\ +\ J & = \min \sum_{k=1}^{n-2} ||(\mathbf{p}_{k+1} - \mathbf{p}_{k}) - (\mathbf{p}_{k} - \mathbf{p}_{k-1})||^2 \\\ +\ & = \min \sum_{k=1}^{n-2} ||\mathbf{p}_{k+1} - 2 \mathbf{p}_{k} + \mathbf{p}_{k-1}||^2 \\\ +\ & = \min \sum_{k=1}^{n-2} \{(x_{k+1} - x_k + x_{k-1})^2 + (y_{k+1} - y_k + y_{k-1})^2\} \\\ \ & = \min \begin{pmatrix} - \ x_0 \\ - \ x_1 \\ - \ x_2 \\ - \vdots \\ - \ x_{n-3}\\ - \ x_{n-2} \\ - \ x_{n-1} \\ - \ y_0 \\ - \ y_1 \\ - \ y_2 \\ - \vdots \\ - \ y_{n-3}\\ - \ y_{n-2} \\ - \ y_{n-1} \\ + \ x_0 \\\ + \ x_1 \\\ + \ x_2 \\\ + \vdots \\\ + \ x_{n-3}\\\ + \ x_{n-2} \\\ + \ x_{n-1} \\\ + \ y_0 \\\ + \ y_1 \\\ + \ y_2 \\\ + \vdots \\\ + \ y_{n-3}\\\ + \ y_{n-2} \\\ + \ y_{n-1} \\\ \end{pmatrix}^T \begin{pmatrix} - 1 & -2 & 1 & 0 & \dots& \\ - -2 & 5 & -4 & 1 & 0 &\dots \\ - 1 & -4 & 6 & -4 & 1 & \\ - 0 & 1 & -4 & 6 & -4 & \\ - \vdots & 0 & \ddots&\ddots& \ddots \\ - & \vdots & & & \\ - & & & 1 & -4 & 6 & -4 & 1 \\ - & & & & 1 & -4 & 5 & -2 \\ - & & & & & 1 & -2 & 1& \\ - & & & & & & & &1 & -2 & 1 & 0 & \dots& \\ - & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\ - & & & & & & & &1 & -4 & 6 & -4 & 1 & \\ - & & & & & & & &0 & 1 & -4 & 6 & -4 & \\ - & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\ - & & & & & & & & & \vdots & & & \\ - & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\ - & & & & & & & & & & & & 1 & -4 & 5 & -2 \\ - & & & & & & & & & & & & & 1 & -2 & 1& \\ + 1 & -2 & 1 & 0 & \dots& \\\ + -2 & 5 & -4 & 1 & 0 &\dots \\\ + 1 & -4 & 6 & -4 & 1 & \\\ + 0 & 1 & -4 & 6 & -4 & \\\ + \vdots & 0 & \ddots&\ddots& \ddots \\\ + & \vdots & & & \\\ + & & & 1 & -4 & 6 & -4 & 1 \\\ + & & & & 1 & -4 & 5 & -2 \\\ + & & & & & 1 & -2 & 1& \\\ + & & & & & & & &1 & -2 & 1 & 0 & \dots& \\\ + & & & & & & & &-2 & 5 & -4 & 1 & 0 &\dots \\\ + & & & & & & & &1 & -4 & 6 & -4 & 1 & \\\ + & & & & & & & &0 & 1 & -4 & 6 & -4 & \\\ + & & & & & & & &\vdots & 0 & \ddots&\ddots& \ddots \\\ + & & & & & & & & & \vdots & & & \\\ + & & & & & & & & & & & 1 & -4 & 6 & -4 & 1 \\\ + & & & & & & & & & & & & 1 & -4 & 5 & -2 \\\ + & & & & & & & & & & & & & 1 & -2 & 1& \\\ \end{pmatrix} \begin{pmatrix} - \ x_0 \\ - \ x_1 \\ - \ x_2 \\ - \vdots \\ - \ x_{n-3}\\ - \ x_{n-2} \\ - \ x_{n-1} \\ - \ y_0 \\ - \ y_1 \\ - \ y_2 \\ - \vdots \\ - \ y_{n-3}\\ - \ y_{n-2} \\ - \ y_{n-1} \\ + \ x_0 \\\ + \ x_1 \\\ + \ x_2 \\\ + \vdots \\\ + \ x_{n-3}\\\ + \ x_{n-2} \\\ + \ x_{n-1} \\\ + \ y_0 \\\ + \ y_1 \\\ + \ y_2 \\\ + \vdots \\\ + \ y_{n-3}\\\ + \ y_{n-2} \\\ + \ y_{n-1} \\\ \end{pmatrix} \end{align} $$ 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_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp similarity index 57% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 3e6aafd3a9954..c1d92c06a8e05 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -12,35 +12,44 @@ // 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_ +#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_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 "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_optimizer +namespace static_centerline_generator { -using static_centerline_optimizer::srv::LoadMap; -using static_centerline_optimizer::srv::PlanPath; -using static_centerline_optimizer::srv::PlanRoute; +using static_centerline_generator::srv::LoadMap; +using static_centerline_generator::srv::PlanPath; +using static_centerline_generator::srv::PlanRoute; -class StaticCenterlineOptimizerNode : public rclcpp::Node +struct CenterlineWithRoute +{ + std::vector centerline{}; + std::vector route_lane_ids{}; +}; + +class StaticCenterlineGeneratorNode : public rclcpp::Node { public: - explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options); + explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); void run(); private: @@ -55,45 +64,45 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node 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; + // 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 std::vector & route_lane_ids, - const std::vector & optimized_traj_points); + 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}; - int traj_start_index_{0}; - int traj_end_index_{0}; - struct MetaDataToSaveMap - { - std::vector optimized_traj_points{}; - std::vector route_lane_ids{}; + std::pair traj_range_indices_{0, 0}; + std::optional centerline_with_route_{std::nullopt}; + + enum class CenterlineSource { + OptimizationTrajectoryBase = 0, + BagEgoTrajectoryBase, }; - std::optional meta_data_to_save_map_{std::nullopt}; + CenterlineSource centerline_source_; + OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; // 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_whole_optimized_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_optimized_centerline_{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_; @@ -106,5 +115,5 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node // vehicle info vehicle_info_util::VehicleInfo vehicle_info_; }; -} // namespace static_centerline_optimizer -#endif // STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ +} // 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 86% 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 2e163e2eb93eb..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,7 +55,7 @@ - + @@ -77,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 94% rename from planning/static_centerline_optimizer/package.xml rename to planning/static_centerline_generator/package.xml index eef133581622f..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 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_optimizer/scripts/centerline_updater_helper.py b/planning/static_centerline_generator/scripts/centerline_updater_helper.py similarity index 99% rename from planning/static_centerline_optimizer/scripts/centerline_updater_helper.py rename to planning/static_centerline_generator/scripts/centerline_updater_helper.py index 00a646406f14f..fec3d21d20bec 100755 --- a/planning/static_centerline_optimizer/scripts/centerline_updater_helper.py +++ b/planning/static_centerline_generator/scripts/centerline_updater_helper.py @@ -155,7 +155,7 @@ def __init__(self): transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) self.sub_whole_centerline = self.create_subscription( Trajectory, - "/static_centerline_optimizer/output_whole_centerline", + "/static_centerline_generator/output_whole_centerline", self.onWholeCenterline, transient_local_qos, ) diff --git a/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py new file mode 100755 index 0000000000000..912226511f1d9 --- /dev/null +++ b/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py @@ -0,0 +1,139 @@ +#!/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 argparse +from decimal import Decimal +import os +import subprocess +import xml.etree.ElementTree as ET + + +def sort_attributes(root): + for shallow_element in root: + # sort nodes + attrib = shallow_element.attrib + if len(attrib) > 1: + attributes = sorted(attrib.items()) + attrib.clear() + attrib.update(attributes) + if shallow_element.tag == "relation": + pass + + # sort the element in the tag + for deep_element in shallow_element: + attrib = deep_element.attrib + if len(attrib) > 1: + # adjust attribute order, e.g. by sorting + attributes = sorted(attrib.items()) + attrib.clear() + attrib.update(attributes) + + # sort tags + sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items()) + if len(shallow_element) != 0: + # NOTE: This "tail" is related to the indent of the next tag + first_tail = shallow_element[0].tail + last_tail = shallow_element[-1].tail + for idx, val_shallow_element in enumerate(sorted_shallow_element): + shallow_element[idx] = val_shallow_element + if idx == len(shallow_element) - 1: + shallow_element[idx].tail = last_tail + else: + shallow_element[idx].tail = first_tail + + +def remove_diff_to_ignore(osm_root): + decimal_precision = 11 # for lat/lon values + + # remove attributes of the osm tag + osm_root.attrib.clear() + + # remove the MetaInfo tag generated by Vector Map Builder + if osm_root[0].tag == "MetaInfo": + osm_root.remove(osm_root[0]) + + # remove unnecessary attributes for diff + for osm_child in osm_root: + if osm_child.tag == "osm": + osm_child.attrib.pop("osm") + if "visible" in osm_child.attrib and osm_child.attrib["visible"]: + osm_child.attrib.pop("visible") + if "version" in osm_child.attrib and osm_child.attrib["version"]: + osm_child.attrib.pop("version") + if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify": + osm_child.attrib.pop("action") + if "lat" in osm_child.attrib: + osm_child.attrib["lat"] = str( + Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}")) + ) + if "lon" in osm_child.attrib: + osm_child.attrib["lon"] = str( + Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}")) + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps" + ) + parser.add_argument( + "-i", + "--ignore-minor-attributes", + action="store_true", + help="Ignore minor attributes of LL2 maps which does not change the map's behavior", + ) + args = parser.parse_args() + + original_osm_file_name = "/tmp/static_centerline_generator/input/lanelet2_map.osm" + modified_osm_file_name = "/tmp/static_centerline_generator/output/lanelet2_map.osm" + + # load LL2 maps + original_osm_tree = ET.parse(original_osm_file_name) + original_osm_root = original_osm_tree.getroot() + modified_osm_tree = ET.parse(modified_osm_file_name) + modified_osm_root = modified_osm_tree.getroot() + + # sort attributes + if args.sort_attributes: + sort_attributes(modified_osm_root) + sort_attributes(original_osm_root) + + # ignore minor attributes + if args.ignore_minor_attributes: + remove_diff_to_ignore(original_osm_root) + remove_diff_to_ignore(modified_osm_root) + + # write LL2 maps + output_dir_path = "/tmp/static_centerline_generator/show_lanelet2_map_diff/" + os.makedirs(output_dir_path + "original/", exist_ok=True) + os.makedirs(output_dir_path + "modified/", exist_ok=True) + + original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm") + modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm") + + # show diff + print("[INFO] Show diff of following LL2 maps") + print(f" {output_dir_path + 'original/lanelet2_map.osm'}") + print(f" {output_dir_path + 'modified/lanelet2_map.osm'}") + subprocess.run( + [ + "diff", + output_dir_path + "original/lanelet2_map.osm", + output_dir_path + "modified/lanelet2_map.osm", + "--color", + ] + ) 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 55% 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 ea7e1c302fd68..750b2e68b16b0 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.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" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" @@ -21,11 +21,10 @@ #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" @@ -35,6 +34,7 @@ #include #include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" #include @@ -47,57 +47,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; @@ -109,28 +69,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; @@ -175,7 +114,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); @@ -209,127 +148,202 @@ std::vector check_lanelet_connection( return unconnected_lane_ids; } -std_msgs::msg::Header createHeader(const rclcpp::Time & now) +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_whole_optimized_centerline_ = - create_publisher("output_whole_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) { - if (!meta_data_to_save_map_ || traj_end_index_ + 1 < msg.data) { - return; - } - traj_start_index_ = msg.data; - - const auto & d = meta_data_to_save_map_; - const auto selected_optimized_traj_points = std::vector( - d->optimized_traj_points.begin() + traj_start_index_, - d->optimized_traj_points.begin() + traj_end_index_ + 1); - - pub_optimized_centerline_->publish(motion_utils::convertToTrajectory( - selected_optimized_traj_points, createHeader(this->now()))); + 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) { - if (!meta_data_to_save_map_ || msg.data + 1 < traj_start_index_) { - return; - } - traj_end_index_ = msg.data; - - const auto & d = meta_data_to_save_map_; - const auto selected_optimized_traj_points = std::vector( - d->optimized_traj_points.begin() + traj_start_index_, - d->optimized_traj_points.begin() + traj_end_index_ + 1); - - pub_optimized_centerline_->publish(motion_utils::convertToTrajectory( - selected_optimized_traj_points, createHeader(this->now()))); + 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 (!meta_data_to_save_map_ || msg.data) { - const auto & d = meta_data_to_save_map_; - const auto selected_optimized_traj_points = std::vector( - d->optimized_traj_points.begin() + traj_start_index_, - d->optimized_traj_points.begin() + traj_end_index_ + 1); - save_map(lanelet2_output_file_path, d->route_lane_ids, selected_optimized_traj_points); + 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); - traj_start_index_ = 0; - traj_end_index_ = optimized_traj_points.size() - 1; - 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{}; + } - meta_data_to_save_map_ = MetaDataToSaveMap{optimized_traj_points, route_lane_ids}; + // 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) { + // copy the input LL2 map to the temporary file for debugging + const std::string debug_input_file_dir{"/tmp/static_centerline_generator/input/"}; + std::filesystem::create_directories(debug_input_file_dir); + std::filesystem::copy( + lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", + std::filesystem::copy_options::overwrite_existing); + // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map @@ -375,7 +389,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"; @@ -396,7 +410,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_) { @@ -421,7 +435,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 @@ -436,7 +450,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_) { @@ -450,7 +464,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; @@ -469,134 +483,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_whole_optimized_centerline_->publish( - motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header)); - 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_) { @@ -607,7 +494,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); @@ -619,7 +506,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()) { @@ -638,8 +527,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++; @@ -652,7 +541,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); @@ -667,23 +556,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; @@ -740,22 +626,29 @@ 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_, *map_projector_); RCLCPP_INFO(get_logger(), "Saved map."); + + // copy the output LL2 map to the temporary file for debugging + const std::string debug_output_file_dir{"/tmp/static_centerline_generator/output/"}; + std::filesystem::create_directories(debug_output_file_dir); + std::filesystem::copy( + lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", + std::filesystem::copy_options::overwrite_existing); } -} // 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/scripts/tmp/run.sh b/planning/static_centerline_optimizer/scripts/tmp/run.sh deleted file mode 100755 index 9e475b9d8759b..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/autoware_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