diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 6433f1b9ae2f9..b6d10bbf40de5 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -16,7 +16,8 @@ #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #include "motion_utils/resample/resample_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include #include diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 331b3dd5275fd..afead3a3614d4 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -18,8 +18,6 @@ #include "motion_utils/marker/marker_helper.hpp" #include -#include -#include #include #include diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 25d8616949b02..c869271222387 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,6 +14,10 @@ #include "motion_utils/marker/marker_helper.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +#include + #include using tier4_autoware_utils::appendMarkerArray; diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 63c456ab24fb0..114c4f87907c0 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -14,6 +14,8 @@ #include "motion_utils/marker/virtual_wall_marker_creator.hpp" +#include + namespace motion_utils { diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index 85578d3c18dbc..aa20dd1ffc7ce 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -15,7 +15,6 @@ #include "path_distance_calculator.hpp" #include -#include #include #include diff --git a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..ed458cf924e33 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_target_object_type_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(OpenCV REQUIRED) +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/target_object_type_panel.hpp + src/target_object_type_panel.cpp +) +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${OpenCV_LIBRARIES} +) +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_target_object_type_rviz_plugin/README.md b/common/tier4_target_object_type_rviz_plugin/README.md new file mode 100644 index 0000000000000..1296bd3442ab3 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/README.md @@ -0,0 +1,9 @@ +# tier4_target_object_type_rviz_plugin + +This plugin allows you to check which types of the dynamic object is being used by each planner. + +![window](./image/window.png) + +## Limitations + +Currently, which parameters of which module to check are hardcoded. In the future, this will be parameterized using YAML. diff --git a/common/tier4_target_object_type_rviz_plugin/image/window.png b/common/tier4_target_object_type_rviz_plugin/image/window.png new file mode 100644 index 0000000000000..33aa9a1e5a26e Binary files /dev/null and b/common/tier4_target_object_type_rviz_plugin/image/window.png differ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml new file mode 100644 index 0000000000000..6209a5aaaee6b --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/package.xml @@ -0,0 +1,27 @@ + + + + tier4_target_object_type_rviz_plugin + 0.0.1 + The tier4_target_object_type_rviz_plugin package + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_rendering + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..eb3350e4333bd --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + TargetObjectTypePanel + + + diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp new file mode 100644 index 0000000000000..cf5960cac281f --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp @@ -0,0 +1,248 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "target_object_type_panel.hpp" + +#include +#include +#include +#include +#include + +TargetObjectTypePanel::TargetObjectTypePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + node_ = std::make_shared("matrix_display_node"); + + setParameters(); + + matrix_widget_ = new QTableWidget(modules_.size(), targets_.size(), this); + for (size_t i = 0; i < modules_.size(); i++) { + matrix_widget_->setVerticalHeaderItem( + i, new QTableWidgetItem(QString::fromStdString(modules_[i]))); + } + for (size_t j = 0; j < targets_.size(); j++) { + matrix_widget_->setHorizontalHeaderItem( + j, new QTableWidgetItem(QString::fromStdString(targets_[j]))); + } + + updateMatrix(); + + reload_button_ = new QPushButton("Reload", this); + connect( + reload_button_, &QPushButton::clicked, this, &TargetObjectTypePanel::onReloadButtonClicked); + + QVBoxLayout * layout = new QVBoxLayout; + layout->addWidget(matrix_widget_); + layout->addWidget(reload_button_); + setLayout(layout); +} + +void TargetObjectTypePanel::onReloadButtonClicked() +{ + RCLCPP_INFO(node_->get_logger(), "Reload button clicked. Update parameter data."); + updateMatrix(); +} + +void TargetObjectTypePanel::setParameters() +{ + // Parameter will be investigated for these modules: + modules_ = { + "avoidance", + "avoidance_by_lane_change", + "lane_change", + "obstacle_cruise (inside)", + "obstacle_cruise (outside)", + "obstacle_stop", + "obstacle_slowdown"}; + + // Parameter will be investigated for targets in each module + targets_ = {"car", "truck", "bus", "trailer", "unknown", "bicycle", "motorcycle", "pedestrian"}; + + // TODO(Horibe): If the param naming strategy is aligned, this should be done automatically based + // on the modules_ and targets_. + + // default + ParamNameEnableObject default_param_name; + default_param_name.name.emplace("car", "car"); + default_param_name.name.emplace("truck", "truck"); + default_param_name.name.emplace("bus", "bus"); + default_param_name.name.emplace("trailer", "trailer"); + default_param_name.name.emplace("unknown", "unknown"); + default_param_name.name.emplace("bicycle", "bicycle"); + default_param_name.name.emplace("motorcycle", "motorcycle"); + default_param_name.name.emplace("pedestrian", "pedestrian"); + + // avoidance + { + const auto module = "avoidance"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance.target_object"; + param_name.name.emplace("car", "car.is_target"); + param_name.name.emplace("truck", "truck.is_target"); + param_name.name.emplace("bus", "bus.is_target"); + param_name.name.emplace("trailer", "trailer.is_target"); + param_name.name.emplace("unknown", "unknown.is_target"); + param_name.name.emplace("bicycle", "bicycle.is_target"); + param_name.name.emplace("motorcycle", "motorcycle.is_target"); + param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_names_.emplace(module, param_name); + } + + // avoidance_by_lane_change + { + const auto module = "avoidance_by_lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance_by_lane_change.target_object"; + param_name.name.emplace("car", "car.is_target"); + param_name.name.emplace("truck", "truck.is_target"); + param_name.name.emplace("bus", "bus.is_target"); + param_name.name.emplace("trailer", "trailer.is_target"); + param_name.name.emplace("unknown", "unknown.is_target"); + param_name.name.emplace("bicycle", "bicycle.is_target"); + param_name.name.emplace("motorcycle", "motorcycle.is_target"); + param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_names_.emplace(module, param_name); + } + + // lane_change + { + const auto module = "lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "lane_change.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (inside) + { + const auto module = "obstacle_cruise (inside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.inside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (outside) + { + const auto module = "obstacle_cruise (outside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.outside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle stop + { + const auto module = "obstacle_stop"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.stop_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle slowdown + { + const auto module = "obstacle_slowdown"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.slow_down_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } +} + +void TargetObjectTypePanel::updateMatrix() +{ + // blue base + // const QColor color_in_use("#6eb6cc"); + // const QColor color_no_use("#1d3e48"); + // const QColor color_undefined("#9e9e9e"); + + // green base + const QColor color_in_use("#afff70"); + const QColor color_no_use("#44642b"); + const QColor color_undefined("#9e9e9e"); + + const auto set_undefined = [&](const auto i, const auto j) { + QTableWidgetItem * item = new QTableWidgetItem("N/A"); + item->setForeground(QBrush(Qt::black)); // set the text color to black + item->setBackground(color_undefined); + matrix_widget_->setItem(i, j, item); + }; + + for (size_t i = 0; i < modules_.size(); i++) { + const auto & module = modules_[i]; + + // Check if module exists in param_names_ + if (param_names_.find(module) == param_names_.end()) { + RCLCPP_WARN_STREAM(node_->get_logger(), module << " is not in the param names"); + continue; + } + + const auto & module_params = param_names_.at(module); + auto parameters_client = + std::make_shared(node_, module_params.node); + if (!parameters_client->wait_for_service(std::chrono::microseconds(500))) { + RCLCPP_WARN_STREAM( + node_->get_logger(), "Failed to find parameter service for node: " << module_params.node); + for (size_t j = 0; j < targets_.size(); j++) { + set_undefined(i, j); + } + continue; + } + + for (size_t j = 0; j < targets_.size(); j++) { + const auto & target = targets_[j]; + + // Check if target exists in module's name map + if (module_params.name.find(target) == module_params.name.end()) { + RCLCPP_WARN_STREAM( + node_->get_logger(), target << " parameter is not set in the " << module); + continue; + } + + std::string param_name = module_params.ns + "." + module_params.name.at(target); + auto parameter_result = parameters_client->get_parameters({param_name}); + + if (!parameter_result.empty()) { + bool value = parameter_result[0].as_bool(); + QTableWidgetItem * item = new QTableWidgetItem(value ? "O" : "X"); + item->setForeground(QBrush(value ? Qt::black : Qt::black)); // set the text color to black + item->setBackground(QBrush(value ? color_in_use : color_no_use)); + matrix_widget_->setItem(i, j, item); + } else { + RCLCPP_WARN_STREAM( + node_->get_logger(), + "Failed to get parameter " << module_params.node << " " << param_name); + + set_undefined(i, j); + } + } + } +} + +PLUGINLIB_EXPORT_CLASS(TargetObjectTypePanel, rviz_common::Panel) diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp new file mode 100644 index 0000000000000..1f3934369bfba --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TARGET_OBJECT_TYPE_PANEL_HPP_ +#define TARGET_OBJECT_TYPE_PANEL_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +class TargetObjectTypePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit TargetObjectTypePanel(QWidget * parent = 0); + +protected: + QTableWidget * matrix_widget_; + std::shared_ptr node_; + std::vector modules_; + std::vector targets_; + + struct ParamNameEnableObject + { + std::string node; + std::string ns; + std::unordered_map name; + }; + std::unordered_map param_names_; + +private slots: + void onReloadButtonClicked(); + +private: + QPushButton * reload_button_; + + void updateMatrix(); + void setParameters(); +}; + +#endif // TARGET_OBJECT_TYPE_PANEL_HPP_ diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index e71003ea7f61c..cba0fa16b50fe 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -25,7 +25,8 @@ #include #include #include -#include +#include +#include #include #endif diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 7e10ca9864f11..6eff270e78326 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -15,7 +15,9 @@ #include "autonomous_emergency_braking/node.hpp" #include -#include +#include +#include +#include #include #include diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index 2aade12d5f1cf..1b11d1e55c376 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -44,7 +44,8 @@ void insertPointInPredictedTrajectory( TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) { - TrajectoryPoints reversed_trajectory_points(trajectory_points.size()); + TrajectoryPoints reversed_trajectory_points; + reversed_trajectory_points.reserve(trajectory_points.size()); std::reverse_copy( trajectory_points.begin(), trajectory_points.end(), std::back_inserter(reversed_trajectory_points)); @@ -106,7 +107,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN // trajectory: t1--------//------tN // ↓ - // predicted_trajectory: 　　　　 tNew--p3----//------pN + // predicted_trajectory: 　　　　 pNew--p3----//------pN // trajectory: t1--------//------tN auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( trajectory_points, modified_trajectory_points, predicted_trajectory_points); diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 9822fd820dc3c..573a38593fc59 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 643f8a6f91023..82ebadafa17bf 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -14,8 +14,10 @@ #include "mpc_lateral_controller/mpc.hpp" -#include "motion_utils/motion_utils.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index af680b5050037..0f815a482f80d 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -14,7 +14,7 @@ #include "mpc_lateral_controller/mpc_lateral_controller.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 891a48299c81d..6d6e980675086 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -16,7 +16,7 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/normalization.hpp" diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f8434bbdb081f..a17e491ebe98d 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -16,8 +16,9 @@ #include "util.hpp" -#include -#include +#include +#include +#include #include #include diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 337a63bc7dc76..40385f2853983 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,8 +14,9 @@ #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index f073574cc229f..5e9488bf2ca93 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -14,8 +14,8 @@ #include "trajectory_follower_node/simple_trajectory_follower.hpp" -#include -#include +#include +#include #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index aeace3234d6b9..78d91e266c516 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,7 +20,7 @@ #include "vehicle_cmd_filter.hpp" #include -#include +#include #include #include #include diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp index 2a22692dc5ac9..2bb0dad86bf71 100644 --- a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -14,8 +14,6 @@ #include "kinematic_evaluator/metrics/kinematic_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - namespace kinematic_diagnostics { namespace metrics diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp index 240a7dd91e9b2..fab3377913bd4 100644 --- a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp +++ b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp @@ -14,7 +14,7 @@ #include "localization_evaluator/metrics/localization_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include namespace localization_diagnostics { diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index 5146fcd3ec0f2..e1ad9c1a2eeec 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -15,7 +15,8 @@ #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" namespace planning_diagnostics { diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp index e2fd04a1d7a6e..6134a100f31a4 100644 --- a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index 4d97757c3c33e..82d6dcc51097e 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -14,7 +14,7 @@ #include "planning_evaluator/metrics/obstacle_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp index abd3ca5584951..7a3418da881c6 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp @@ -15,7 +15,7 @@ #include "planning_evaluator/metrics/stability_metrics.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index cba03e8b2d90e..d0d8d61c4c231 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -15,8 +15,7 @@ #include "planning_evaluator/metrics/trajectory_metrics.hpp" #include "planning_evaluator/metrics/metrics_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp index da04d6fcd2bfa..38113513dbad2 100644 --- a/evaluator/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp @@ -14,13 +14,12 @@ #include "planning_evaluator/metrics_calculator.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "planning_evaluator/metrics/obstacle_metrics.hpp" #include "planning_evaluator/metrics/stability_metrics.hpp" #include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index b0c7a5c4471c6..97d4a02472f2d 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,8 +15,8 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 79083b755fb26..74c340ca53557 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -35,6 +35,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { ; // do nothing } else { diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0352096d02b2b..f62e418371401 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp + src/utils/start_goal_planner_common/utils.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 1609cdbc60b7a..160ebdc180020 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -21,12 +21,14 @@ rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint left: 0.5 # [m] extra length to add to the left of the dynamic object footprint right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + path_preprocessing: + max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) expansion: method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. # 'lanelet': add lanelets overlapped by the ego footprints # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 5b252cef92714..9217e139d218c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -148,6 +149,7 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable std::optional drivable_area_expansion_prev_crop_pose; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 29857590d4eda..b77ac16a9db61 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -364,16 +364,10 @@ class PlannerManager * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet( - const std::shared_ptr & data, bool success_lane_change = false) const + lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const { lanelet::ConstLanelet ret{}; - if (success_lane_change) { - data->route_handler->getClosestPreferredLaneletWithinRoute( - data->self_odometry->pose.pose, &ret); - } else { - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); - } + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 964e0ff87594a..1ddf288e06ae3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -65,7 +65,7 @@ enum class PathType { FREESPACE, }; -struct PUllOverStatus +struct PullOverStatus { std::shared_ptr pull_over_path{}; std::shared_ptr lane_parking_pull_over_path{}; @@ -76,7 +76,8 @@ struct PUllOverStatus lanelet::ConstLanelets pull_over_lanes{}; std::vector lanes{}; // current + pull_over bool has_decided_path{false}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; @@ -134,7 +135,7 @@ class GoalPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } - PUllOverStatus status_; + PullOverStatus status_; std::shared_ptr parameters_; @@ -205,7 +206,6 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - boost::optional calcFeasibleDecelDistance(const double target_velocity) const; void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index e2934a02b63d8..0dc07f0950716 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -70,7 +70,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 53ef91473fe24..8293e0a1d10a9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" @@ -22,17 +23,17 @@ #include +#include + namespace drivable_area_expansion { /// @brief Expand the drivable area based on the projected ego footprint along the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] params expansion parameters -/// @param[in] dynamic_objects dynamic objects -/// @param[in] route_handler route handler +/// @param[inout] path path whose drivable area will be expanded +/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) /// @param[in] path_lanes lanelets of the path void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes); /// @brief Create a polygon combining the drivable area of a path and some expansion polygons diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index e5e1c76f2521f..8fc0157710dc8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -64,6 +64,6 @@ multi_polygon_t createObjectFootprints( /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); + const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index a4c11c68d13ec..81eab9560b736 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -48,8 +48,10 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto RESAMPLE_INTERVAL_PARAM = + "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = - "dynamic_expansion.expansion.max_path_arc_length"; + "dynamic_expansion.path_preprocessing.max_arc_length"; static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; @@ -78,6 +80,7 @@ struct DrivableAreaExpansionParameters double dynamic_objects_extra_front_offset{}; double max_expansion_distance{}; double max_path_arc_length{}; + double resample_interval{}; double extra_arc_length{}; bool avoid_dynamic_objects{}; std::vector avoid_linestring_types{}; @@ -98,6 +101,7 @@ struct DrivableAreaExpansionParameters enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); + resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index daabfa97598fd..e300a347e47a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -26,6 +26,7 @@ namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index cb769a1f2884c..9cae5e18f1352 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -94,6 +94,10 @@ class GeometricParallelParking std::vector getArcPaths() const { return arc_paths_; } std::vector getPaths() const { return paths_; } + std::vector> getPairsTerminalVelocityAndAccel() const + { + return pairs_terminal_velocity_and_accel_; + } PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; @@ -112,6 +116,7 @@ class GeometricParallelParking std::vector arc_paths_; std::vector paths_; + std::vector> pairs_terminal_velocity_and_accel_; size_t current_path_idx_ = 0; void clearPaths(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..6b8a62d2caec9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -99,6 +100,18 @@ struct GoalPlannerParameters AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; + // debug bool print_debug_info; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4b800917d4aec..20788e53309ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -46,6 +47,8 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 93eb0457d7cf4..475b38f5bb824 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -170,7 +170,7 @@ struct ObjectsFilteringParams struct SafetyCheckParams { bool enable_safety_check; ///< Enable safety checks. - double backward_lane_length; ///< Length of the backward lane for path generation. + double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp new file mode 100644 index 0000000000000..4ba2cb08b6341 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ + +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +/* + * Common data for start/goal_planner module + */ +struct StartGoalPlannerData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp new file mode 100644 index 0000000000000..60bbb3c78b401 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -0,0 +1,94 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity); + +/** + * @brief Update path velocities based on driving direction. + * + * This function updates the longitudinal velocity of each point in the provided paths, + * based on whether the vehicle is driving forward or backward. It also sets the terminal + * velocity and acceleration for each path. + * + * @param paths A vector of paths with lane IDs to be updated. + * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal + * velocity and acceleration for a corresponding path. + * @param target_velocity The target velocity for ego vehicle predicted path. + * @param acceleration The acceleration for ego vehicle predicted path. + */ +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel); + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx); + +} // namespace behavior_path_planner::utils::start_goal_planner_common + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 1e26bef0c85be..771fbd93f7196 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -76,6 +77,18 @@ struct StartPlannerParameters PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; }; } // namespace behavior_path_planner 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 f86b2ff2c9c61..a9b137e54e90f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1044,6 +1044,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); + updateParam( + parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, + planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.extra_arc_length); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 920b44235cba9..e6e2bf9ac5346 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -580,7 +580,7 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname().find("lane_change") != std::string::npos; }); if (success_lane_change) { - root_lanelet_ = updateRootLanelet(data, true); + root_lanelet_ = updateRootLanelet(data); } std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index c47aa66fdac90..1c1b327e717eb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -332,7 +332,7 @@ ObjectData AvoidanceModule::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -983,6 +983,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { avoid_lines.push_back(al_avoid); avoid_lines.push_back(al_return); + } else { + o.reason = "InvalidShiftLine"; + continue; } o.is_avoidable = true; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 6c7853ac0e54f..1d491a7fb7b87 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -34,6 +35,7 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; @@ -117,7 +119,7 @@ GoalPlannerModule::GoalPlannerModule( void GoalPlannerModule::resetStatus() { - PUllOverStatus initial_status{}; + PullOverStatus initial_status{}; status_ = initial_status; pull_over_path_candidates_.clear(); closest_start_pose_.reset(); @@ -342,7 +344,8 @@ bool GoalPlannerModule::isExecutionReady() const double GoalPlannerModule::calcModuleRequestLength() const { - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return parameters_->minimum_request_length; } @@ -442,7 +445,7 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); status_.pull_over_path = std::make_shared(*freespace_path); status_.current_path_idx = 0; - status_.is_safe = true; + status_.is_safe_static_objects = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); debug_data_.freespace_planner.is_planning = false; @@ -479,7 +482,7 @@ void GoalPlannerModule::returnToLaneParking() } mutex_.lock(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.has_decided_path = false; status_.pull_over_path = status_.lane_parking_pull_over_path; status_.current_path_idx = 0; @@ -541,7 +544,7 @@ void GoalPlannerModule::selectSafePullOverPath() const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); - status_.is_safe = false; + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -560,7 +563,7 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe = true; + status_.is_safe_static_objects = true; mutex_.lock(); status_.pull_over_path = std::make_shared(pull_over_path); status_.current_path_idx = 0; @@ -572,7 +575,7 @@ void GoalPlannerModule::selectSafePullOverPath() } // decelerate before the search area start - if (status_.is_safe) { + if (status_.is_safe_static_objects) { const auto search_start_offset_pose = calcLongitudinalOffsetPose( status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - @@ -583,7 +586,9 @@ void GoalPlannerModule::selectSafePullOverPath() decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance(parameters_->pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -616,7 +621,7 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe) { + if (status_.is_safe_static_objects) { // clear stop pose when the path is safe and activated if (isActivated()) { resetWallPoses(); @@ -645,7 +650,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe; + status_.prev_is_safe = status_.is_safe_static_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -694,7 +699,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe) { + if (status_.is_safe_static_objects) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose; @@ -742,7 +747,7 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return false; } @@ -864,7 +869,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe + path_candidate_ = status_.is_safe_static_objects ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -959,17 +964,19 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { + if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() - : *search_start_offset_pose); + const Pose stop_pose = + status_.is_safe_static_objects + ? status_.pull_over_path->start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -986,7 +993,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); for (auto & p : reference_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1014,7 +1023,8 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return stop_path; } @@ -1099,7 +1109,7 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1221,7 +1231,8 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - const auto current_to_stop_distance = calcFeasibleDecelDistance(0.0); + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -1257,30 +1268,6 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional GoalPlannerModule::calcFeasibleDecelDistance( - const double target_velocity) const -{ - const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; - const auto a_now = planner_data_->self_acceleration->accel.accel.linear.x; - const auto a_lim = parameters_->maximum_deceleration; // positive value - const auto j_lim = parameters_->maximum_jerk; - - if (v_now < target_velocity) { - return 0.0; - } - - auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( - v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim); - - if (!min_stop_distance) { - return {}; - } - - min_stop_distance = std::max(*min_stop_distance, 0.0); - - return min_stop_distance; -} - double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { @@ -1305,7 +1292,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const float decel_vel = std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); - const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -1323,7 +1311,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { const auto stop_point = utils::insertStopPoint(stop_point_length, path); @@ -1337,7 +1326,9 @@ void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -1479,7 +1470,7 @@ void GoalPlannerModule::setDebugData() } // Visualize path and related pose - if (status_.is_safe) { + if (status_.is_safe_static_objects) { add(createPoseMarkerArray( status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -1499,8 +1490,8 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 841fc9092e279..a8ff0c0bbcf02 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -216,7 +216,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 64a7e926cc2df..74af83c2db2a1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -88,14 +88,11 @@ ModuleStatus LaneChangeInterface::updateState() } if (!module_type_->isValidPath()) { - return ModuleStatus::RUNNING; + return ModuleStatus::SUCCESS; } if (module_type_->isAbortState()) { - if (module_type_->hasFinishedAbort()) { - resetLaneChangeModule(); - } - return ModuleStatus::RUNNING; + return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING; } if (module_type_->hasFinishedLaneChange()) { @@ -154,10 +151,7 @@ ModuleStatus LaneChangeInterface::updateState() getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); module_type_->toCancelState(); - if (!isWaitingApproval()) { - resetLaneChangeModule(); - } - return ModuleStatus::RUNNING; + return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; } if (!module_type_->isAbortEnabled()) { @@ -192,13 +186,6 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } -void LaneChangeInterface::resetLaneChangeModule() -{ - processOnExit(); - removeRTCStatus(); - processOnEntry(); -} - void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 856c3220165e7..945344259fdb2 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -406,13 +406,14 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( } double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose) + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); + const auto idx = findFirstNearestIndex(path.points, point); + const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { if (lateral > largest_overhang) { @@ -1014,7 +1015,7 @@ void filterTargetObjects( const auto lines = rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = distance2d(to2D(overhang_basic_pose), to2D(line.basicLineString())); + const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); if (d < o.to_road_shoulder_distance) { o.to_road_shoulder_distance = d; target_line = line; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 2b62d5e7c3b53..0fcb0acfcb000 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -19,20 +19,71 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "interpolation/linear_interpolation.hpp" #include +#include +#include #include namespace drivable_area_expansion { +std::vector crop_and_resample( + const std::vector & points, + const std::shared_ptr planner_data, + const double resample_interval) +{ + auto lon_offset = 0.0; + auto crop_pose = *planner_data->drivable_area_expansion_prev_crop_pose; + // reuse or update the previous crop point + if (planner_data->drivable_area_expansion_prev_crop_pose) { + const auto lon_offset = motion_utils::calcSignedArcLength( + points, points.front().point.pose.position, crop_pose.position); + if (lon_offset < 0.0) { + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } else { + const auto is_behind_ego = + motion_utils::calcSignedArcLength( + points, crop_pose.position, planner_data->self_odometry->pose.pose.position) > 0.0; + const auto is_too_far = motion_utils::calcLateralOffset(points, crop_pose.position) > 0.1; + if (!is_behind_ego || is_too_far) + planner_data->drivable_area_expansion_prev_crop_pose.reset(); + } + } + if (!planner_data->drivable_area_expansion_prev_crop_pose) { + crop_pose = planner_data->drivable_area_expansion_prev_crop_pose.value_or( + motion_utils::calcInterpolatedPose(points, resample_interval - lon_offset)); + } + // crop + const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); + const auto cropped_points = motion_utils::cropPoints( + points, crop_pose.position, crop_seg_idx + 1, + planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); + planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; + // resample + PathWithLaneId cropped_path; + if (tier4_autoware_utils::calcDistance2d(crop_pose, cropped_points.front()) > 1e-3) { + PathPointWithLaneId crop_path_point; + crop_path_point.point.pose = crop_pose; + cropped_path.points.push_back(crop_path_point); + } + cropped_path.points.insert( + cropped_path.points.end(), cropped_points.begin(), cropped_points.end()); + const auto resampled_path = + motion_utils::resamplePath(cropped_path, resample_interval, true, true, false); + + return resampled_path.points; +} + void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, + PathWithLaneId & path, + const std::shared_ptr planner_data, const lanelet::ConstLanelets & path_lanes) { + const auto & params = planner_data->drivable_area_expansion_parameters; + const auto & dynamic_objects = *planner_data->dynamic_object; + const auto & route_handler = *planner_data->route_handler; const auto uncrossable_lines = extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); multi_linestring_t uncrossable_lines_in_range; @@ -40,7 +91,8 @@ void expandDrivableArea( for (const auto & line : uncrossable_lines) if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) uncrossable_lines_in_range.push_back(line); - const auto path_footprints = createPathFootprints(path, params); + const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); + const auto path_footprints = createPathFootprints(points, params); const auto predicted_paths = createObjectFootprints(dynamic_objects, params); const auto expansion_polygons = params.expansion_method == "lanelet" @@ -113,8 +165,8 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ { const auto original_left_bound = path.left_bound; const auto original_right_bound = path.right_bound; - const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { - return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; + const auto is_left_of_path = [&](const point_t & p) { + return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0; }; // prepare delimiting lines: start and end of the original expanded drivable area const auto start_segment = @@ -178,12 +230,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ *it, *std::next(it), start_segment.first, start_segment.second); if (inter_start) { const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left_of_path_start = is_left_of_segment( - convert_point(path.points[0].point.pose.position), - convert_point(path.points[1].point.pose.position), *inter_start); - if (is_left_of_path_start && dist < start_left.distance) + const auto is_left = is_left_of_path(*inter_start); + if (is_left && dist < start_left.distance) start_left.update(*inter_start, it, dist); - else if (!is_left_of_path_start && dist < start_right.distance) + else if (!is_left && dist < start_right.distance) start_right.update(*inter_start, it, dist); } const auto inter_end = @@ -192,11 +242,10 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); if (inter_end) { const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left_of_path_end = is_left_of_segment( - convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end); - if (is_left_of_path_end && dist < end_left.distance) + const auto is_left = is_left_of_path(*inter_end); + if (is_left && dist < end_left.distance) end_left.update(*inter_end, it, dist); - else if (!is_left_of_path_end && dist < end_right.distance) + else if (!is_left && dist < end_right.distance) end_right.update(*inter_end, it, dist); } } diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 5304985b1cbf2..828fdc2f17a51 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -85,7 +85,7 @@ std::array calculate_arc_length_range_and_distance( } for (const auto & p : footprint.outer()) { const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; + if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { expansion_dist = std::abs(projection.distance); from_arc_length = std::min(from_arc_length, projection.arc_length); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 45e128288578a..4a45dce6a0bcc 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -66,7 +66,7 @@ multi_polygon_t createObjectFootprints( } multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params) + const std::vector & points, const DrivableAreaExpansionParameters & params) { const auto left = params.ego_left_offset + params.ego_extra_left_offset; const auto right = params.ego_right_offset - params.ego_extra_right_offset; @@ -78,9 +78,9 @@ multi_polygon_t createPathFootprints( point_t{front, left}}; multi_polygon_t footprints; // skip the last footprint as its orientation is usually wrong - footprints.reserve(path.points.size() - 1); + footprints.reserve(points.size() - 1); double arc_length = 0.0; - for (auto it = path.points.begin(); std::next(it) != path.points.end(); ++it) { + for (auto it = points.begin(); std::next(it) != points.end(); ++it) { footprints.push_back(createFootprint(it->point.pose, base_footprint)); if (params.max_path_arc_length > 0.0) { arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index a5ccfc9517771..27dbfb532c1fc 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -163,6 +163,7 @@ void GeometricParallelParking::clearPaths() current_path_idx_ = 0; arc_paths_.clear(); paths_.clear(); + pairs_terminal_velocity_and_accel_.clear(); } bool GeometricParallelParking::planPullOver( @@ -182,6 +183,7 @@ bool GeometricParallelParking::planPullOver( if (is_forward) { // When turning forward to the right, the front left goes out, // so reduce the steer angle at that time for seach no lane departure path. + // TODO(Sugahara): define in the config constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; @@ -481,6 +483,19 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + } else { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + } + // set pull_over start and end pose // todo: make start and end pose for pull_out start_pose_ = start_pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index f4aabb13b8916..9656094f5a478 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include #include @@ -55,8 +56,7 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - const bool found_path = planner_->makePlan(current_pose, end_pose); - if (!found_path) { + if (!planner_->makePlan(current_pose, end_pose)) { return {}; } @@ -113,18 +113,21 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) addPose(goal_pose); } - utils::correctDividedPathVelocity(partial_paths); + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::start_goal_planner_common::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); - if (!is_driving_forward) { - // path points is less than 2 + if (!motion_utils::isDrivingForward(path.points)) { return {}; } } PullOverPath pull_over_path{}; pull_over_path.partial_paths = partial_paths; + pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; pull_over_path.start_pose = current_pose; pull_over_path.end_pose = goal_pose; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 6a5ccc827db29..7760b9b57d8ab 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -75,6 +75,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 2f5c1d9b05b7f..c5ac14d4d9727 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -204,6 +204,7 @@ boost::optional ShiftPullOver::generatePullOverPath( PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths.push_back(shifted_path.path); + pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index a775e7c16efed..23016b01cb9ab 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -343,13 +343,13 @@ TargetObjectsOnLane createTargetObjectsOnLane( }; // TODO(Sugahara): Consider shoulder and other lane objects - if (object_lane_configuration.check_current_lane) { + if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } - if (object_lane_configuration.check_left_lane) { + if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); } - if (object_lane_configuration.check_right_lane) { + if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ec14a064bf51b..c4bfb62276d42 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -418,6 +418,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; } diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp new file mode 100644 index 0000000000000..f29711c82b3ce --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -0,0 +1,148 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity) +{ + const auto v_now = planner_data->self_odometry->twist.twist.linear.x; + const auto a_now = planner_data->self_acceleration->accel.accel.linear.x; + + if (v_now < target_velocity) { + return 0.0; + } + + auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( + v_now, target_velocity, a_now, -acc_lim, jerk_lim, -1.0 * jerk_lim); + + if (!min_stop_distance) { + return {}; + } + + min_stop_distance = std::max(*min_stop_distance, 0.0); + + return min_stop_distance; +} + +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration) +{ + assert(paths.size() == terminal_vel_acc_pairs.size()); + + auto path_itr = std::begin(paths); + auto pair_itr = std::begin(terminal_vel_acc_pairs); + + for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { + const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + + // If the number of points in the path is less than 2, don't insert stop velocity and + // set pairs_terminal_velocity_and_accel to 0 + if (!is_driving_forward) { + *pair_itr = std::make_pair(0.0, 0.0); + continue; + } + + if (*is_driving_forward) { + for (auto & point : path_itr->points) { + // TODO(Sugahara): velocity calculation can be improved by considering the acceleration + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + // TODO(Sugahara): Consider the calculation of the target velocity and acceleration for ego's + // predicted path when ego will stop at the end of the path + *pair_itr = std::make_pair(target_velocity, acceleration); + } else { + for (auto & point : path_itr->points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + *pair_itr = std::make_pair(-target_velocity, -acceleration); + } + path_itr->points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel) +{ + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; +} + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx) +{ + if (pairs_terminal_velocity_and_accel.size() <= current_path_idx) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel.at(current_path_idx); +} + +} // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 45d36d51d69b4..29abdb08c5c55 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -69,11 +69,10 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); + const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, @@ -123,6 +122,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index eb2859ff700af..dbfc86e5b8ba5 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1532,9 +1532,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea( - path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, - transformed_lanes); + drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); } // make bound longitudinally monotonic diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index fe33d48f74fe5..7a5bb68decdfa 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" @@ -257,6 +258,7 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit + params.resample_interval = 1.0; params.extra_arc_length = 1.0; params.expansion_method = "polygon"; // 2m x 4m ego footprint @@ -265,9 +267,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.ego_left_offset = 2.0; params.ego_right_offset = -2.0; } + behavior_path_planner::PlannerData planner_data; + planner_data.drivable_area_expansion_parameters = params; + planner_data.reference_path = std::make_shared(path); + planner_data.dynamic_object = + std::make_shared(dynamic_objects); + planner_data.route_handler = std::make_shared(route_handler); // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expandDrivableArea( - path, params, dynamic_objects, route_handler, path_lanes); + path, std::make_shared(planner_data), path_lanes); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { @@ -276,13 +284,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) } // expanded left bound - ASSERT_EQ(path.left_bound.size(), 3ul); + ASSERT_EQ(path.left_bound.size(), 4ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[3].x, 2.0, eps); + EXPECT_NEAR(path.left_bound[3].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 418f05baeaf20..2c57ff9458833 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,17 +14,20 @@ #include "manager.hpp" +#include + #include namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 1.5); - planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info", false); + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); } void NoDrivableLaneModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index dd4c1c610261c..510dc86ef651d 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -34,6 +34,7 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego extra_front_offset: 0.0 # [m] extra front distance extra_rear_offset: 0.0 # [m] extra rear distance extra_right_offset: 0.0 # [m] extra right distance diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 163c8632e40c7..46db7a53b5c81 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -30,11 +30,11 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx) ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } -double time_along_path(const EgoData & ego_data, const size_t target_idx) +double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) { const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( - ego_data.velocity, + std::max(ego_data.velocity, min_velocity), ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; @@ -55,8 +55,7 @@ bool object_is_incoming( std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger) + const std::shared_ptr route_handler, const rclcpp::Logger & logger) { // skip the dynamic object if it is not in a lane preceding the overlapped lane // lane changes are intentionally not considered @@ -65,13 +64,12 @@ std::optional> object_time_to_range( object.kinematics.initial_pose_with_covariance.pose.position.y); if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - const auto max_deviation = object.shape.dimensions.y * 2.0; + const auto max_deviation = object.shape.dimensions.y + range.inside_distance; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { - if (predicted_path.confidence < min_confidence) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -281,8 +279,10 @@ bool should_not_enter( const rclcpp::Logger & logger) { RangeTimes range_times{}; - range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx); - range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx); + range_times.ego.enter_time = + time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); RCLCPP_DEBUG( logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); @@ -298,8 +298,7 @@ bool should_not_enter( // skip objects that are already on the interval const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_min_confidence, logger) + ? object_time_to_range(object, range, inputs.route_handler, logger) : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index bf6bf81e506cf..1d1fe8bea94a6 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -48,6 +48,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief estimate the time when ego will reach some target path index /// @param [in] ego_data data related to the ego vehicle /// @param [in] target_idx target ego path index +/// @param [in] min_velocity minimum ego velocity used to estimate the time /// @return time taken by ego to reach the target [s] double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief use an object's predicted paths to estimate the times it will reach the enter and exit @@ -57,14 +58,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @param [in] object dynamic object /// @param [in] range overlapping range /// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] min_confidence minimum confidence to consider a predicted path /// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger); + const std::shared_ptr route_handler, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..48869e5e3926d --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +/// @brief filter predicted objects and their predicted paths +/// @param [in] objects predicted objects to filter +/// @param [in] ego_data ego data +/// @param [in] params parameters +/// @return filtered predicted objects +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = objects.header; + for (const auto & object : objects.objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + } + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace behavior_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index a0e2b86caa623..9924319b141f7 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -54,16 +54,21 @@ lanelet::ConstLanelets calculate_other_lanelets( const auto lanelets_within_range = lanelet::geometry::findWithin2d( route_handler.getLaneletMapPtr()->laneletLayer, ego_point, std::max(params.slow_dist_threshold, params.stop_dist_threshold)); + const auto is_overlapped_by_path_lanelets = [&](const auto & l) { + for (const auto & path_ll : path_lanelets) { + if ( + boost::geometry::overlaps( + path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; for (const auto & ll : lanelets_within_range) { const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - const auto is_overlapped_by_path_lanelets = [&](const auto & l) { - for (const auto & path_ll : path_lanelets) - if (boost::geometry::overlaps( - path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon())) - return true; - return false; - }; if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) other_lanelets.push_back(ll.second); } diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 075738f07d62e..5765363024aed 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -64,6 +64,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.stop_dist_threshold = getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); + pp.ego_min_velocity = getOrDeclareParameter(node, ns + ".ego.min_assumed_velocity"); pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns + ".ego.extra_left_offset"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 5922a5b9b4488..430cec2d3d4ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -16,6 +16,7 @@ #include "debug.hpp" #include "decisions.hpp" +#include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" #include "overlapping_range.hpp" @@ -103,7 +104,7 @@ bool OutOfLaneModule::modifyPathVelocity( DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = *planner_data_->predicted_objects; + inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; auto decisions = calculate_decisions(inputs, params_, logger_); diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 85266d779f34e..b81fa09884819 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -42,10 +42,11 @@ struct PlannerParam double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // # whether to use the objects' predicted paths - double objects_min_vel; // # [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // # minimum confidence to consider a predicted path + bool objects_use_predicted_paths; // whether to use the objects' predicted paths + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index eb405f06c8c4b..6756a82ca20d4 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -79,7 +79,8 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config("stop_line"), get_behavior_velocity_module_config("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane")}); + get_behavior_velocity_module_config("out_of_lane"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 3ce69aa265090..f942d9a00c098 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -40,6 +40,13 @@ help="Options VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk", ) +parser.add_argument( + "-v", + "--max-velocity", + type=int, + help="maximum plotting velocity in Matplotlib", +) + args = parser.parse_args() PLOT_MIN_ARCLENGTH = -5 @@ -61,6 +68,11 @@ PLOT_TYPE = "VEL" print("plot type = " + PLOT_TYPE) +if args.max_velocity is None: + MAX_VELOCITY = 20 +else: + MAX_VELOCITY = args.max_velocity + class TrajectoryVisualizer(Node): def __init__(self): @@ -274,6 +286,7 @@ def setPlotTrajectoryVelocity(self): self.ax1.set_title("trajectory's velocity") self.ax1.legend() self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) + self.ax1.set_ylim([0, MAX_VELOCITY]) self.ax1.set_ylabel("vel [m/s]") return ( @@ -398,9 +411,6 @@ def plotTrajectoryVelocity(self, data): if len(y) != 0: self.min_vel = np.min(y) - # change y-range - self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0]) - return ( self.im1, self.im2, @@ -577,6 +587,7 @@ def setPlotTrajectory(self): self.ax1.set_title("trajectory's velocity") self.ax1.legend() self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) + self.ax1.set_ylim([0, MAX_VELOCITY]) self.ax1.set_ylabel("vel [m/s]") self.ax2 = plt.subplot(3, 1, 2) diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 993cf2d9da9e1..bd9886d376cad 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 3487e9515c48a..b271f75eeeb6c 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | @@ -33,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | `gnss_frame` | string | "gnss" | frame id | | `gnss_base_frame` | string | "gnss_base_link" | frame id | | `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System | | `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml deleted file mode 100644 index e931e5adceb68..0000000000000 --- a/sensing/gnss_poser/config/set_local_origin.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - - # Latitude of local origin - latitude: 40.81187906 - - # Longitude of local origin - longitude: 29.35810110 - - # Altitude of local origin - altitude: 47.62 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 4dda5d1276c54..5cd0767f28130 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -42,7 +43,7 @@ enum class MGRSPrecision { GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system) + const std::string & target_vertical_datum) { GNSSStat utm; @@ -50,16 +51,9 @@ GNSSStat NavSatFix2UTM( GeographicLib::UTMUPS::Forward( nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, utm.y); - - std::string target_height_system; - if (height_system == 0) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } utm.z = geography_utils::convert_height( nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", - target_height_system); + target_vertical_datum); utm.latitude = nav_sat_fix_msg.latitude; utm.longitude = nav_sat_fix_msg.longitude; utm.altitude = nav_sat_fix_msg.altitude; @@ -71,24 +65,20 @@ GNSSStat NavSatFix2UTM( GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) + const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger, + const std::string & target_vertical_datum) { GNSSStat utm_local; try { // origin of the local coordinate system in global frame GNSSStat utm_origin; GeographicLib::UTMUPS::Forward( - nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, + geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); - std::string target_height_system; - if (height_system == 0) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } + utm_origin.z = geography_utils::convert_height( - nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, - "WGS84", target_height_system); + geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84", + target_vertical_datum); // individual coordinates of global coordinate system double global_x = 0.0; @@ -104,7 +94,7 @@ GNSSStat NavSatFix2LocalCartesianUTM( utm_local.y = global_y - utm_origin.y; utm_local.z = geography_utils::convert_height( nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, - "WGS84", target_height_system) - + "WGS84", target_vertical_datum) - utm_origin.z; } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( @@ -143,9 +133,9 @@ GNSSStat UTM2MGRS( GNSSStat NavSatFix2MGRS( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system) + const rclcpp::Logger & logger, const std::string & vertical_datum) { - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); + const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, vertical_datum); const auto mgrs = UTM2MGRS(utm, precision, logger); return mgrs; } diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 4baaec2dfe80e..05acfa967eb96 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -17,6 +17,8 @@ #include "gnss_poser/convert.hpp" #include "gnss_poser/gnss_stat.hpp" +#include +#include #include #include @@ -48,6 +50,9 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callbackGnssInsOrientationStamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); @@ -55,8 +60,8 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); GNSSStat convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system); + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + const MapProjectorInfo::Message & projector_info); geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); geometry_msgs::msg::Point getMedianPosition( const boost::circular_buffer & position_buffer); @@ -81,6 +86,7 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformListener tf2_listener_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; rclcpp::Subscription::SharedPtr autoware_orientation_sub_; @@ -89,20 +95,18 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr fixed_pub_; - CoordinateSystem coordinate_system_; + MapProjectorInfo::Message projector_info_; std::string base_frame_; std::string gnss_frame_; std::string gnss_base_frame_; std::string map_frame_; - - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; + bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; boost::circular_buffer position_buffer_; autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int height_system_; int gnss_pose_pub_method; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index d33bd022c3714..acd21a83d96e4 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -18,8 +18,6 @@ namespace gnss_poser { -enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 }; - struct GNSSStat { GNSSStat() diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 8a0ffb8cab8c3..60e952c6845f4 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -2,7 +2,6 @@ - @@ -13,10 +12,8 @@ - - @@ -31,11 +28,8 @@ - - - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 77380bf5492a1..7d80bc903a550 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -14,6 +14,9 @@ libboost-dev autoware_sensing_msgs + component_interface_specs + component_interface_utils + geographic_msgs geographiclib geography_utils geometry_msgs diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 4f0723944913b..9cd5a3ad34098 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -34,16 +34,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), msg_gnss_ins_orientation_stamped_( std::make_shared()), - height_system_(declare_parameter("height_system", 1)), gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) { - int coordinate_system = - declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); - coordinate_system_ = static_cast(coordinate_system); - - nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0); - nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0); - nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); int buff_epoch = declare_parameter("buff_epoch", 1); position_buffer_.set_capacity(buff_epoch); @@ -61,9 +58,24 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); } +void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + projector_info_ = *msg; + received_map_projector_info_ = true; +} + void GNSSPoser::callbackNavSatFix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { + // Return immediately if map_projector_info has not been received yet. + if (!received_map_projector_info_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "map_projector_info has not been received yet. Check if the map_projection_loader is " + "successfully launched."); + return; + } + // check fixed topic const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); @@ -80,8 +92,8 @@ void GNSSPoser::callbackNavSatFix( return; } - // get position in coordinate_system - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_); + // get position + const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_); const auto position = getPosition(gnss_stat); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -186,20 +198,22 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix } GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system) + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + const MapProjectorInfo::Message & map_projector_info) { GNSSStat gnss_stat; - if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { + if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::MGRS) { + nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), + map_projector_info.vertical_datum); + } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) { gnss_stat = NavSatFix2MGRS( - nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); + nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), + map_projector_info.vertical_datum); } else { RCLCPP_ERROR_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Unknown Coordinate System"); + "Unknown Projector type"); } return gnss_stat; } diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 6036e760fed47..f4cb6063a9bf6 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -14,7 +14,7 @@ #include "dummy_perception_publisher/node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index cfc58cdaeed63..5c8a7eabdab86 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -17,7 +17,9 @@ #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 46c8ea4125c9a..5df76e9b5f3cc 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -14,6 +14,8 @@ #include "planning.hpp" +#include + #include #include #include diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp index 03b45fd3b5d41..b533390dcc6fb 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/default_ad_api/src/planning.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include