diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 57b3b8263aa27..df2f0c379c265 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -10,6 +10,7 @@ common/autoware_point_types/** taichi.higashide@tier4.jp @autowarefoundation/aut common/autoware_testing/** adam.dabrowski@robotec.ai @autowarefoundation/autoware-global-codeowners common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners +common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/fake_test_node/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners @@ -18,7 +19,7 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp @autowarefoundation/aut common/grid_map_utils/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/motion_utils/** satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -27,7 +28,7 @@ common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/auto common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/tensorrt_common/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -44,17 +45,18 @@ common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/ common/time_utils/** christopherj.ho@gmail.com @autowarefoundation/autoware-global-codeowners common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners common/tvm_utility/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners -common/web_controller/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai @autowarefoundation/autoware-global-codeowners control/external_cmd_selector/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners control/joy_controller/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners control/lane_departure_checker/** kyoichi.sugahara@tier4.jp @autowarefoundation/autoware-global-codeowners +control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/obstacle_collision_checker/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/pure_pursuit/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners control/shift_decider/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -control/trajectory_follower/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -control/trajectory_follower_nodes/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners +control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners +control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners @@ -94,9 +96,9 @@ perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muram perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners -perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/map_based_prediction/** yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners perception/multi_object_tracker/** jilada.eccleston@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/object_merger/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/object_range_splitter/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -112,9 +114,9 @@ perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp @autoware perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/costmap_generator/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/freespace_planner/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/mission_planner/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -152,6 +154,7 @@ system/bluetooth_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-g system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners +system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners system/dummy_diag_publisher/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners system/dummy_infrastructure/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -165,6 +168,6 @@ system/velodyne_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-gl tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai @autowarefoundation/autoware-global-codeowners vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -vehicle/raw_vehicle_cmd_converter/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp new file mode 100644 index 0000000000000..4d4530dc38b75 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp @@ -0,0 +1,46 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_AD_API_SPECS__PLANNING_HPP_ +#define AUTOWARE_AD_API_SPECS__PLANNING_HPP_ + +#include + +#include +#include + +namespace autoware_ad_api::planning +{ + +struct VelocityFactors +{ + using Message = autoware_adapi_v1_msgs::msg::VelocityFactorArray; + static constexpr char name[] = "/api/planning/velocity_factors"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct SteeringFactors +{ + using Message = autoware_adapi_v1_msgs::msg::SteeringFactorArray; + static constexpr char name[] = "/api/planning/steering_factors"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::planning + +#endif // AUTOWARE_AD_API_SPECS__PLANNING_HPP_ diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index a1a39f5cd57af..488a18b40c99c 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -19,6 +19,7 @@ #include #include +#include namespace localization_interface { @@ -38,6 +39,15 @@ struct InitializationState static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; +struct KinematicState +{ + using Message = nav_msgs::msg::Odometry; + static constexpr char name[] = "/localization/kinematic_state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + } // namespace localization_interface #endif // COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index c3106cf7b60ad..fecee4426cc11 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -62,6 +63,15 @@ struct Route static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; +struct Trajectory +{ + using Message = autoware_auto_planning_msgs::msg::Trajectory; + static constexpr char name[] = "/planning/scenario_planning/trajectory"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + } // namespace planning_interface #endif // COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ diff --git a/common/component_interface_tools/CMakeLists.txt b/common/component_interface_tools/CMakeLists.txt new file mode 100644 index 0000000000000..a5ebc29463bec --- /dev/null +++ b/common/component_interface_tools/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(component_interface_tools) + +find_package(autoware_cmake REQUIRED) +autoware_package() +ament_auto_add_executable(service_log_checker src/service_log_checker.cpp) +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/common/component_interface_tools/launch/service_log_checker.launch.xml b/common/component_interface_tools/launch/service_log_checker.launch.xml new file mode 100644 index 0000000000000..f3099b3238096 --- /dev/null +++ b/common/component_interface_tools/launch/service_log_checker.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml new file mode 100644 index 0000000000000..a1eba3cb41e09 --- /dev/null +++ b/common/component_interface_tools/package.xml @@ -0,0 +1,28 @@ + + + + component_interface_tools + 0.1.0 + The component_interface_tools package + Takagi, Isamu + yabuta + Kah Hooi Tan + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + diagnostic_updater + fmt + rclcpp + tier4_system_msgs + yaml_cpp_vendor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/component_interface_tools/src/service_log_checker.cpp b/common/component_interface_tools/src/service_log_checker.cpp new file mode 100644 index 0000000000000..ce89573356412 --- /dev/null +++ b/common/component_interface_tools/src/service_log_checker.cpp @@ -0,0 +1,110 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "service_log_checker.hpp" + +#include + +#include +#include + +#define FMT_HEADER_ONLY +#include + +ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this) +{ + sub_ = create_subscription( + "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1)); + + diagnostics_.setHardwareID(get_name()); + diagnostics_.add("response_status", this, &ServiceLogChecker::update_diagnostics); +} + +void ServiceLogChecker::on_service_log(const ServiceLog::ConstSharedPtr msg) +{ + try { + // Ignore service request. + if (msg->type == ServiceLog::CLIENT_REQUEST || msg->type == ServiceLog::SERVER_REQUEST) { + return; + } + + // Ignore service errors. + if (msg->type == ServiceLog::ERROR_UNREADY) { + return set_error(*msg, "not ready"); + } + if (msg->type == ServiceLog::ERROR_TIMEOUT) { + return set_error(*msg, "timeout"); + } + + // Ignore version API because it doesn't have response status. + if (msg->name == "/api/interface/version") { + return; + } + + // Parse response data. + const auto status = YAML::Load(msg->yaml)["status"]; + if (!status) { + return set_error(*msg, "no response status"); + } + + // Check response status. + const auto success = status["success"].as(); + if (!success) { + const auto message = status["message"].as(); + const auto code = status["code"].as(); + return set_error(*msg, fmt::format("status code {} '{}'", code, message)); + } + } catch (const YAML::Exception & error) { + return set_error(*msg, fmt::format("invalid data: '{}'", error.what())); + } + + set_success(*msg); +} + +void ServiceLogChecker::set_success(const ServiceLog & msg) +{ + errors_.erase(fmt::format("{} ({})", msg.name, msg.node)); +} + +void ServiceLogChecker::set_error(const ServiceLog & msg, const std::string & log) +{ + errors_[fmt::format("{} ({})", msg.name, msg.node)] = log; + RCLCPP_ERROR_STREAM(get_logger(), fmt::format("{}: {} ({})", msg.name, log, msg.node)); +} + +void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + for (const auto & error : errors_) { + stat.add(error.first, error.second); + } + + if (errors_.empty()) { + stat.summary(DiagnosticStatus::OK, "OK"); + } else { + stat.summary(DiagnosticStatus::ERROR, "ERROR"); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/common/component_interface_tools/src/service_log_checker.hpp b/common/component_interface_tools/src/service_log_checker.hpp new file mode 100644 index 0000000000000..32c7f02e757c6 --- /dev/null +++ b/common/component_interface_tools/src/service_log_checker.hpp @@ -0,0 +1,42 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SERVICE_LOG_CHECKER_HPP_ +#define SERVICE_LOG_CHECKER_HPP_ + +#include +#include + +#include + +#include +#include + +class ServiceLogChecker : public rclcpp::Node +{ +public: + ServiceLogChecker(); + +private: + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + rclcpp::Subscription::SharedPtr sub_; + diagnostic_updater::Updater diagnostics_; + void on_service_log(const ServiceLog::ConstSharedPtr msg); + void set_success(const ServiceLog & msg); + void set_error(const ServiceLog & msg, const std::string & log); + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + std::unordered_map errors_; +}; + +#endif // SERVICE_LOG_CHECKER_HPP_ diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d753b4e92d218..d4f6343e381c7 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -3,5 +3,4 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() -ament_export_dependencies(tier4_system_msgs) ament_auto_package() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index 78620dab76cfe..90602a65bd1b0 100644 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + tier4_system_msgs autoware_adapi_v1_msgs rclcpp diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 2a119fa02177d..f93e758c47332 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace motion_utils @@ -580,6 +581,43 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } +template +inline std::vector calcCurvature(const T & points) +{ + std::vector curvature_vec(points.size()); + + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); + curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3)); + } + curvature_vec.at(0) = curvature_vec.at(1); + curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); + + return curvature_vec; +} + +template +inline std::vector> calcCurvatureAndArcLength(const T & points) +{ + // Note that arclength is for the segment, not the sum. + std::vector> curvature_arc_length_vec; + curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); + const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); + const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + curvature_arc_length_vec.push_back(std::pair(curvature, arc_length)); + } + curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + + return curvature_arc_length_vec; +} + /** * @brief Calculate distance to the forward stop point from the given src index */ diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index d8f6340a473e4..ee6975e0caa83 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -8,7 +8,11 @@ Satoshi Ota Takayuki Murooka + Fumiya Watanabe + Kosuke Takeuchi Taiki Tanaka + Takamasa Horibe + Tomoya Kimura Apache License 2.0 Yutaka Shimizu diff --git a/common/tensorrt_common/CMakeLists.txt b/common/tensorrt_common/CMakeLists.txt index 1714c5a44f7eb..395a5ee4f1941 100644 --- a/common/tensorrt_common/CMakeLists.txt +++ b/common/tensorrt_common/CMakeLists.txt @@ -4,6 +4,9 @@ project(tensorrt_common) find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(tensorrt_common): Remove once upgrading to TensorRT 8.5 is complete +add_compile_options(-Wno-deprecated-declarations) + find_package(CUDA) find_package(CUDNN) find_package(TENSORRT) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp new file mode 100644 index 0000000000000..5b2f197c18827 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp @@ -0,0 +1,34 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ + +#include + +#include + +namespace tier4_autoware_utils +{ +inline std::string toHexString(const unique_identifier_msgs::msg::UUID & id) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; + } + return ss.str(); +} +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index 35b4c7b836b46..b1cb540ee7a77 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -33,6 +33,7 @@ #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include "tier4_autoware_utils/ros/wait_for_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 1e9acb6101935..9e648c49e067b 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -6,6 +6,8 @@ The tier4_autoware_utils package Takamasa Horibe Kenji Miyake + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 ament_cmake_auto @@ -23,6 +25,7 @@ tf2 tf2_geometry_msgs tier4_debug_msgs + unique_identifier_msgs visualization_msgs ament_cmake_ros diff --git a/common/tvm_utility/include/tvm_utility/pipeline.hpp b/common/tvm_utility/include/tvm_utility/pipeline.hpp index 600d12e1e3ec8..c6ecd9ca15194 100644 --- a/common/tvm_utility/include/tvm_utility/pipeline.hpp +++ b/common/tvm_utility/include/tvm_utility/pipeline.hpp @@ -182,8 +182,21 @@ class Pipeline PostProcessorType post_processor_{}; }; -// Each node should be specified with a string name and a shape -using NetworkNode = std::pair>; +// NetworkNode +typedef struct +{ + // Node name + std::string node_name; + + // Network data type configurations + DLDataTypeCode tvm_dtype_code; + int32_t tvm_dtype_bits; + int32_t tvm_dtype_lanes; + + // Shape info + std::vector node_shape; +} NetworkNode; + typedef struct { // Network info @@ -196,11 +209,6 @@ typedef struct std::string network_graph_path; std::string network_params_path; - // Network data type configurations - DLDataTypeCode tvm_dtype_code; - int32_t tvm_dtype_bits; - int32_t tvm_dtype_lanes; - // Inference hardware configuration DLDeviceType tvm_device_type; int32_t tvm_device_id; @@ -278,8 +286,8 @@ class InferenceEngineTVM : public InferenceEngine for (auto & output_config : config.network_outputs) { output_.push_back(TVMArrayContainer( - output_config.second, config.tvm_dtype_code, config.tvm_dtype_bits, config.tvm_dtype_lanes, - config.tvm_device_type, config.tvm_device_id)); + output_config.node_shape, output_config.tvm_dtype_code, output_config.tvm_dtype_bits, + output_config.tvm_dtype_lanes, config.tvm_device_type, config.tvm_device_id)); } } @@ -290,7 +298,7 @@ class InferenceEngineTVM : public InferenceEngine if (input[index].getArray() == nullptr) { throw std::runtime_error("input variable is null"); } - set_input(config_.network_inputs[index].first.c_str(), input[index].getArray()); + set_input(config_.network_inputs[index].node_name.c_str(), input[index].getArray()); } // Execute the inference diff --git a/common/tvm_utility/test/yolo_v2_tiny/main.cpp b/common/tvm_utility/test/yolo_v2_tiny/main.cpp index 11acf571456b8..aac7900f423c2 100644 --- a/common/tvm_utility/test/yolo_v2_tiny/main.cpp +++ b/common/tvm_utility/test/yolo_v2_tiny/main.cpp @@ -45,18 +45,18 @@ class PreProcessorYoloV2Tiny : public tvm_utility::pipeline::PreProcessor shape_x{1, network_input_width, network_input_height, network_input_depth}; tvm_utility::pipeline::TVMArrayContainer x{ shape_x, - config.tvm_dtype_code, - config.tvm_dtype_bits, - config.tvm_dtype_lanes, + config.network_inputs[0].tvm_dtype_code, + config.network_inputs[0].tvm_dtype_bits, + config.network_inputs[0].tvm_dtype_lanes, config.tvm_device_type, config.tvm_device_id}; @@ -101,7 +101,8 @@ class PreProcessorYoloV2Tiny : public tvm_utility::pipeline::PreProcessor labels{}; std::vector> anchors{}; }; diff --git a/common/tvm_utility/tvm_utility-extras.cmake b/common/tvm_utility/tvm_utility-extras.cmake index 7affa4752b5e4..f346aa8fb4922 100644 --- a/common/tvm_utility/tvm_utility-extras.cmake +++ b/common/tvm_utility/tvm_utility-extras.cmake @@ -14,7 +14,7 @@ # Get user-provided variables set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifacts download") -set(MODELZOO_VERSION "2.1.0-20221102" CACHE STRING "targeted ModelZoo version") +set(MODELZOO_VERSION "3.0.0-20221221" CACHE STRING "targeted ModelZoo version") # # Download the selected neural network if it is not already present on disk. @@ -32,6 +32,7 @@ set(MODELZOO_VERSION "2.1.0-20221102" CACHE STRING "targeted ModelZoo version") function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) set(EXTERNALPROJECT_NAME ${MODEL_NAME}_${MODEL_BACKEND}) + set(PREPROCESSING "") # Prioritize user-provided models. if(IS_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") @@ -42,6 +43,13 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) "${DATA_PATH}/models/${MODEL_NAME}/inference_engine_tvm_config.hpp" COPYONLY ) + if(EXISTS "${DATA_PATH}/user/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") + configure_file( + "${DATA_PATH}/user/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp" + "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp" + COPYONLY + ) + endif() set(DOWNLOAD_DIR "") set(SOURCE_DIR "${DATA_PATH}/user/${MODEL_NAME}") set(INSTALL_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") @@ -63,7 +71,9 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) set(SOURCE_DIR "${DATA_PATH}/models/${MODEL_NAME}") set(INSTALL_DIRECTORY "${DATA_PATH}/models/${MODEL_NAME}") endif() - + if(EXISTS "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") + set(PREPROCESSING "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") + endif() include(ExternalProject) externalproject_add(${EXTERNALPROJECT_NAME} DOWNLOAD_DIR ${DOWNLOAD_DIR} @@ -72,6 +82,7 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) CONFIGURE_COMMAND "" BUILD_COMMAND "" BUILD_BYPRODUCTS "${DATA_PATH}/models/${MODEL_NAME}/inference_engine_tvm_config.hpp" + BUILD_BYPRODUCTS ${PREPROCESSING} INSTALL_COMMAND "" ) install( diff --git a/control/external_cmd_selector/CMakeLists.txt b/control/external_cmd_selector/CMakeLists.txt index 8785c5c5ae811..5c4860b7441f3 100644 --- a/control/external_cmd_selector/CMakeLists.txt +++ b/control/external_cmd_selector/CMakeLists.txt @@ -15,4 +15,5 @@ rclcpp_components_register_node(external_cmd_selector_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/control/external_cmd_selector/config/external_cmd_selector.param.yaml b/control/external_cmd_selector/config/external_cmd_selector.param.yaml new file mode 100644 index 0000000000000..6556656cc8938 --- /dev/null +++ b/control/external_cmd_selector/config/external_cmd_selector.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + update_rate: 10.0 + initial_selector_mode: "local" # ["local", "remote"] diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.py b/control/external_cmd_selector/launch/external_cmd_selector.launch.py index 5e67627207ad3..358cc135996f7 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.py +++ b/control/external_cmd_selector/launch/external_cmd_selector.launch.py @@ -14,25 +14,74 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ComposableNode +import yaml def _create_mapping_tuple(name): return ("~/" + name, LaunchConfiguration(name)) -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): + with open(LaunchConfiguration("external_cmd_selector_param_path").perform(context), "r") as f: + external_cmd_selector_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + component = ComposableNode( + package="external_cmd_selector", + plugin="ExternalCmdSelector", + name="external_cmd_selector", + remappings=[ + _create_mapping_tuple("service/select_external_command"), + _create_mapping_tuple("input/local/control_cmd"), + _create_mapping_tuple("input/local/shift_cmd"), + _create_mapping_tuple("input/local/turn_signal_cmd"), + _create_mapping_tuple("input/local/heartbeat"), + _create_mapping_tuple("input/remote/control_cmd"), + _create_mapping_tuple("input/remote/shift_cmd"), + _create_mapping_tuple("input/remote/turn_signal_cmd"), + _create_mapping_tuple("input/remote/heartbeat"), + _create_mapping_tuple("output/control_cmd"), + _create_mapping_tuple("output/gear_cmd"), + _create_mapping_tuple("output/turn_indicators_cmd"), + _create_mapping_tuple("output/hazard_lights_cmd"), + _create_mapping_tuple("output/heartbeat"), + _create_mapping_tuple("output/current_selector_mode"), + ], + parameters=[ + external_cmd_selector_param, + ], + extra_arguments=[ + { + "use_intra_process_comms": LaunchConfiguration("use_intra_process"), + } + ], + ) + + loader = LoadComposableNodes( + composable_node_descriptions=[component], + target_container=LaunchConfiguration("target_container"), + ) + + group = GroupAction( + [ + PushRosNamespace(""), + loader, + ] + ) + + return [group] + +def generate_launch_description(): arguments = [ # component DeclareLaunchArgument("use_intra_process"), DeclareLaunchArgument("target_container"), - # settings - DeclareLaunchArgument( - "initial_selector_mode", default_value="local", choices=["local", "remote"] - ), # service DeclareLaunchArgument( "service/select_external_command", default_value="~/select_external_command" @@ -82,42 +131,4 @@ def generate_launch_description(): ), ] - component = ComposableNode( - package="external_cmd_selector", - plugin="ExternalCmdSelector", - name="external_cmd_selector", - remappings=[ - _create_mapping_tuple("service/select_external_command"), - _create_mapping_tuple("input/local/control_cmd"), - _create_mapping_tuple("input/local/shift_cmd"), - _create_mapping_tuple("input/local/turn_signal_cmd"), - _create_mapping_tuple("input/local/heartbeat"), - _create_mapping_tuple("input/remote/control_cmd"), - _create_mapping_tuple("input/remote/shift_cmd"), - _create_mapping_tuple("input/remote/turn_signal_cmd"), - _create_mapping_tuple("input/remote/heartbeat"), - _create_mapping_tuple("output/control_cmd"), - _create_mapping_tuple("output/gear_cmd"), - _create_mapping_tuple("output/turn_indicators_cmd"), - _create_mapping_tuple("output/hazard_lights_cmd"), - _create_mapping_tuple("output/heartbeat"), - _create_mapping_tuple("output/current_selector_mode"), - ], - parameters=[ - { - "initial_selector_mode": LaunchConfiguration("initial_selector_mode"), - } - ], - extra_arguments=[ - { - "use_intra_process_comms": LaunchConfiguration("use_intra_process"), - } - ], - ) - - loader = LoadComposableNodes( - composable_node_descriptions=[component], - target_container=LaunchConfiguration("target_container"), - ) - - return LaunchDescription(arguments + [loader]) + return LaunchDescription(arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp index b775a24346bff..0039ec9cd1547 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp @@ -28,8 +28,8 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option using std::placeholders::_2; // Parameter - double update_rate = declare_parameter("update_rate", 10.0); - std::string initial_selector_mode = declare_parameter("initial_selector_mode", "local"); + double update_rate = declare_parameter("update_rate"); + std::string initial_selector_mode = declare_parameter("initial_selector_mode"); // Publisher pub_current_selector_mode_ = diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt new file mode 100644 index 0000000000000..2167805d99c9b --- /dev/null +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(mpc_lateral_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED + src/mpc_lateral_controller.cpp + src/interpolate.cpp + src/lowpass_filter.cpp + src/mpc.cpp + src/mpc_trajectory.cpp + src/mpc_utils.cpp + src/qp_solver/qp_solver_osqp.cpp + src/qp_solver/qp_solver_unconstr_fast.cpp + src/vehicle_model/vehicle_model_bicycle_dynamics.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics.cpp + src/vehicle_model/vehicle_model_interface.cpp +) + +if(BUILD_TESTING) + set(TEST_LAT_SOURCES + test/test_mpc.cpp + test/test_mpc_utils.cpp + test/test_interpolate.cpp + test/test_lowpass_filter.cpp + ) + set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) + ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) + target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB}) +endif() + +ament_auto_package(INSTALL_TO_SHARE + param +) diff --git a/control/trajectory_follower/design/mpc_lateral_controller-design.md b/control/mpc_lateral_controller/README.md similarity index 96% rename from control/trajectory_follower/design/mpc_lateral_controller-design.md rename to control/mpc_lateral_controller/README.md index ee79cc88f206b..7596f753c5edc 100644 --- a/control/trajectory_follower/design/mpc_lateral_controller-design.md +++ b/control/mpc_lateral_controller/README.md @@ -1,7 +1,7 @@ # MPC Lateral Controller This is the design document for the lateral controller node -in the `trajectory_follower_nodes` package. +in the `trajectory_follower_node` package. ## Purpose / Use cases @@ -58,7 +58,7 @@ The tracking is not accurate if the first point of the reference trajectory is a ### Inputs -Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Set the following from the [controller_node](../trajectory_follower_node/README.md) - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry @@ -66,7 +66,7 @@ Set the following from the [controller_node](../../trajectory_follower_nodes/des ### Outputs -Return LateralOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Return LateralOutput which contains the following to the controller node - `autoware_auto_control_msgs/AckermannLateralCommand` - LateralSyncData @@ -93,6 +93,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | path_smoothing_times | int | number of times of applying path smoothing filter | 1 | | curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 | | curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 | +| extend_trajectory_for_end_yaw_control | bool | trajectory extending flag for end yaw control. | true | | steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | | admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 | | admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | diff --git a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp similarity index 82% rename from control/trajectory_follower/include/trajectory_follower/interpolate.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp index 52c26c9330e1a..7f83383a5d65d 100644 --- a/control/trajectory_follower/include/trajectory_follower/interpolate.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp @@ -12,10 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ -#define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ - -#include "trajectory_follower/visibility_control.hpp" +#ifndef MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ +#define MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ #include #include @@ -27,7 +25,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** @@ -37,7 +35,7 @@ namespace trajectory_follower * @param [in] return_index desired interpolated indexes * @param [out] return_value resulting interpolated values */ -TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( +bool linearInterpolate( const std::vector & base_index, const std::vector & base_value, const std::vector & return_index, std::vector & return_value); /** @@ -47,11 +45,11 @@ TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( * @param [in] return_index desired interpolated index * @param [out] return_value resulting interpolated value */ -TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate( +bool linearInterpolate( const std::vector & base_index, const std::vector & base_value, const double & return_index, double & return_value); -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_ +#endif // MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp similarity index 73% rename from control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp index c006c50a6ea2c..e3bd55526adea 100644 --- a/control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp @@ -12,10 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ -#define TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ - -#include "trajectory_follower/visibility_control.hpp" +#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -28,54 +26,13 @@ namespace motion { namespace control { -namespace trajectory_follower -{ - -/** - * @brief Simple filter with gain on the first derivative of the value - */ -class LowpassFilter1d +namespace mpc_lateral_controller { -private: - double m_x; //!< @brief current filtered value - double m_gain; //!< @brief gain value of first-order low-pass filter - -public: - /** - * @brief constructor with initial value and first-order gain - * @param [in] x initial value - * @param [in] gain first-order gain - */ - LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {} - - /** - * @brief set the current value of the filter - * @param [in] x new value - */ - void reset(const double x) { m_x = x; } - - /** - * @brief get the current value of the filter - */ - double getValue() const { return m_x; } - - /** - * @brief filter a new value - * @param [in] u new value - */ - double filter(const double u) - { - const double ret = m_gain * m_x + (1.0 - m_gain) * u; - m_x = ret; - return ret; - } -}; - /** * @brief 2nd-order Butterworth Filter * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930. */ -class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter +class Butterworth2dFilter { private: double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time @@ -149,10 +106,10 @@ namespace MoveAverageFilter * @param [in] num index distance for moving average filter * @param [out] u object vector */ -TRAJECTORY_FOLLOWER_PUBLIC bool filt_vector(const int num, std::vector & u); +bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_ +#endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp similarity index 87% rename from control/trajectory_follower/include/trajectory_follower/mpc.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 8ae6dc5449fc3..5616f26bab641 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -12,26 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__MPC_HPP_ -#define TRAJECTORY_FOLLOWER__MPC_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_HPP_ #include "geometry/common_2d.hpp" #include "helper_functions/angle_utils.hpp" +#include "mpc_lateral_controller/interpolate.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/interpolate.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/mpc_utils.hpp" -#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" -#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" -#include "trajectory_follower/visibility_control.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -50,7 +49,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { struct MPCParam @@ -143,7 +142,7 @@ struct MPCMatrix * MPC-based waypoints follower class * @brief calculate control command to follow reference waypoints */ -class TRAJECTORY_FOLLOWER_PUBLIC MPC +class MPC { private: //!< @brief ROS logger used for debug logging @@ -154,15 +153,15 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC //!< @brief vehicle model type for MPC std::string m_vehicle_model_type; //!< @brief vehicle model for MPC - std::shared_ptr m_vehicle_model_ptr; + std::shared_ptr m_vehicle_model_ptr; //!< @brief qp solver for MPC - std::shared_ptr m_qpsolver_ptr; + std::shared_ptr m_qpsolver_ptr; //!< @brief lowpass filter for steering command - trajectory_follower::Butterworth2dFilter m_lpf_steering_cmd; + mpc_lateral_controller::Butterworth2dFilter m_lpf_steering_cmd; //!< @brief lowpass filter for lateral error - trajectory_follower::Butterworth2dFilter m_lpf_lateral_error; + mpc_lateral_controller::Butterworth2dFilter m_lpf_lateral_error; //!< @brief lowpass filter for heading error - trajectory_follower::Butterworth2dFilter m_lpf_yaw_error; + mpc_lateral_controller::Butterworth2dFilter m_lpf_yaw_error; //!< @brief raw output computed two iterations ago double m_raw_steer_cmd_pprev = 0.0; @@ -185,7 +184,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @brief get variables for mpc calculation */ bool getData( - const trajectory_follower::MPCTrajectory & traj, + const mpc_lateral_controller::MPCTrajectory & traj, const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, const geometry_msgs::msg::Pose & current_pose, MPCData * data); /** @@ -217,14 +216,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @param [out] x updated state at delayed_time */ bool updateStateForDelayCompensation( - const trajectory_follower::MPCTrajectory & traj, const double & start_time, + const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x); /** * @brief generate MPC matrix with trajectory and vehicle model * @param [in] reference_trajectory used for linearization around reference trajectory */ MPCMatrix generateMPCMatrix( - const trajectory_follower::MPCTrajectory & reference_trajectory, const double prediction_dt); + const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt); /** * @brief generate MPC matrix with trajectory and vehicle model * @param [in] mpc_matrix parameters matrix to use for optimization @@ -240,20 +239,20 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ bool resampleMPCTrajectoryByTime( const double start_time, const double prediction_dt, - const trajectory_follower::MPCTrajectory & input, - trajectory_follower::MPCTrajectory * output) const; + const mpc_lateral_controller::MPCTrajectory & input, + mpc_lateral_controller::MPCTrajectory * output) const; /** * @brief apply velocity dynamics filter with v0 from closest index */ - trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter( - const trajectory_follower::MPCTrajectory & trajectory, + mpc_lateral_controller::MPCTrajectory applyVelocityDynamicsFilter( + const mpc_lateral_controller::MPCTrajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const double v0) const; /** * @brief get prediction delta time of mpc. * If trajectory length is shorter than min_prediction length, adjust delta time. */ double getPredictionDeltaTime( - const double start_time, const trajectory_follower::MPCTrajectory & input, + const double start_time, const mpc_lateral_controller::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose) const; /** * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R @@ -349,7 +348,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC public: //!< @brief reference trajectory to be followed - trajectory_follower::MPCTrajectory m_ref_traj; + mpc_lateral_controller::MPCTrajectory m_ref_traj; //!< @brief MPC design parameter MPCParam m_param; //!< @brief mpc_output buffer for delay time compensation @@ -400,12 +399,12 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, const double traj_resample_dist, const bool enable_path_smoothing, const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, - const int curvature_smoothing_num_ref_steer); + const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control); /** * @brief set the vehicle model of this MPC */ inline void setVehicleModel( - std::shared_ptr vehicle_model_ptr, + std::shared_ptr vehicle_model_ptr, const std::string & vehicle_model_type) { m_vehicle_model_ptr = vehicle_model_ptr; @@ -414,7 +413,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /** * @brief set the QP solver of this MPC */ - inline void setQPSolver(std::shared_ptr qpsolver_ptr) + inline void setQPSolver(std::shared_ptr qpsolver_ptr) { m_qpsolver_ptr = qpsolver_ptr; } @@ -445,9 +444,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC */ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; } }; // class MPC -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 79% rename from control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index d38312505fed7..b2c18d0d7d721 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,26 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ -#define TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ - +#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ + +#include "mpc_lateral_controller/interpolate.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "mpc_lateral_controller/mpc.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/interpolate.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/mpc.hpp" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/mpc_utils.hpp" -#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" -#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "trajectory_follower_base/lateral_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -55,12 +54,12 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralControllerBase +class MpcLateralController : public trajectory_follower::LateralControllerBase { public: /** @@ -97,6 +96,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController //!< @brief path resampling interval [m] double m_traj_resample_dist; + //!< @brief flag of traj extending for terminal yaw + bool m_extend_trajectory_for_end_yaw_control; + /* parameters for stop state */ double m_stop_state_entry_ego_speed; double m_stop_state_entry_target_speed; @@ -109,14 +111,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController std::deque m_trajectory_buffer; // MPC object - trajectory_follower::MPC m_mpc; + mpc_lateral_controller::MPC m_mpc; //!< @brief measured kinematic state - nav_msgs::msg::Odometry::SharedPtr m_current_kinematic_state_ptr; + nav_msgs::msg::Odometry m_current_kinematic_state; //!< @brief measured steering - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr; + autoware_auto_vehicle_msgs::msg::SteeringReport m_current_steering; //!< @brief reference trajectory - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr m_current_trajectory_ptr; + autoware_auto_planning_msgs::msg::Trajectory m_current_trajectory; //!< @brief mpc filtered output in previous period double m_steer_cmd_prev = 0.0; @@ -135,20 +137,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController //!< initialize timer to work in real, simulation, and replay void initTimer(double period_s); - /** - * @brief compute control command for path follow with a constant control period - */ - boost::optional run() override; + + bool isReady(const trajectory_follower::InputData & input_data) override; /** - * @brief set input data like current odometry, trajectory and steering. + * @brief compute control command for path follow with a constant control period */ - void setInputData(InputData const & input_data) override; + trajectory_follower::LateralOutput run( + trajectory_follower::InputData const & input_data) override; /** * @brief set m_current_trajectory with received message */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); /** * @brief check if the received data is valid. @@ -211,9 +212,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp similarity index 90% rename from control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index 9016c0adbb689..5abb267eb9d9d 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ -#define TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "trajectory_follower/visibility_control.hpp" #include "geometry_msgs/msg/point.hpp" @@ -29,14 +28,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Trajectory class for mpc follower * @brief calculate control command to follow reference waypoints */ -class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory +class MPCTrajectory { public: std::vector x; //!< @brief x position x vector @@ -97,8 +96,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory return points; } }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_TRAJECTORY_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp similarity index 79% rename from control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 9f394ff9baa6f..f72e7157d6a8f 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ -#define TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "eigen3/Eigen/Core" #include "geometry/common_2d.hpp" @@ -29,9 +29,8 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "trajectory_follower/interpolate.hpp" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/interpolate.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -48,7 +47,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace MPCUtils { @@ -58,19 +57,19 @@ namespace MPCUtils * @param [in] yaw input yaw angle * @return quaternion */ -TRAJECTORY_FOLLOWER_PUBLIC geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw); +geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw); /** * @brief convert Euler angle vector including +-2pi to 0 jump to continuous series data * @param [inout] a input angle vector */ -TRAJECTORY_FOLLOWER_PUBLIC void convertEulerAngleToMonotonic(std::vector * a); +void convertEulerAngleToMonotonic(std::vector * a); /** * @brief calculate the lateral error of the given pose relative to the given reference pose * @param [in] ego_pose pose to check for error * @param [in] ref_pose reference pose * @return lateral distance between the two poses */ -TRAJECTORY_FOLLOWER_PUBLIC double calcLateralError( +double calcLateralError( const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose); /** * @brief convert the given Trajectory msg to a MPCTrajectory object @@ -78,7 +77,7 @@ TRAJECTORY_FOLLOWER_PUBLIC double calcLateralError( * @param [out] output resulting MPCTrajectory * @return true if the conversion was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool convertToMPCTrajectory( +bool convertToMPCTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output); /** * @brief convert the given MPCTrajectory to a Trajectory msg @@ -86,22 +85,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool convertToMPCTrajectory( * @param [out] output resulting Trajectory msg * @return true if the conversion was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool convertToAutowareTrajectory( +bool convertToAutowareTrajectory( const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output); /** * @brief calculate the arc length at each point of the given trajectory * @param [in] trajectory trajectory for which to calculate the arc length * @param [out] arclength the cummulative arc length at each point of the trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC void calcMPCTrajectoryArclength( - const MPCTrajectory & trajectory, std::vector * arclength); +void calcMPCTrajectoryArclength(const MPCTrajectory & trajectory, std::vector * arclength); /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample * @param [in] resample_interval_dist the desired distance between two successive trajectory points * @param [out] output the resampled trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool resampleMPCTrajectoryByDistance( +bool resampleMPCTrajectoryByDistance( const MPCTrajectory & input, const double resample_interval_dist, MPCTrajectory * output); /** * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired @@ -111,7 +109,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool resampleMPCTrajectoryByDistance( * @param [in] out_index desired interpolated indexes * @param [out] out_traj resulting interpolated MPCTrajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpMPCTrajectory( +bool linearInterpMPCTrajectory( const std::vector & in_index, const MPCTrajectory & in_traj, const std::vector & out_index, MPCTrajectory * out_traj); /** @@ -119,7 +117,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpMPCTrajectory( * @param [in] traj MPCTrajectory for which to fill in the relative_time * @return true if the calculation was successful */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcMPCTrajectoryTime(MPCTrajectory & traj); +bool calcMPCTrajectoryTime(MPCTrajectory & traj); /** * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing * @param [in] start_idx index of the trajectory point from which to start smoothing @@ -128,7 +126,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcMPCTrajectoryTime(MPCTrajectory & traj); * @param [in] tau constant to control the smoothing (high-value = very smooth) * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity */ -TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity( +void dynamicSmoothingVelocity( const size_t start_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); /** @@ -136,8 +134,7 @@ TRAJECTORY_FOLLOWER_PUBLIC void dynamicSmoothingVelocity( * @param [inout] traj object trajectory * @param [in] shift is forward or not */ -TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( - MPCTrajectory * traj, const bool is_forward_shift); +void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift); /** * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 * points when num = 1) @@ -146,7 +143,7 @@ TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY( * calculation * @param [inout] traj object trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature( +bool calcTrajectoryCurvature( const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, MPCTrajectory * traj); /** @@ -156,7 +153,7 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature( * @param [in] traj input trajectory * @return vector of curvatures at each point of the given trajectory */ -TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( +std::vector calcTrajectoryCurvature( const size_t curvature_smoothing_num, const MPCTrajectory & traj); /** * @brief calculate nearest pose on MPCTrajectory with linear interpolation @@ -169,7 +166,7 @@ TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( * @param [in] clock to throttle log output * @return false when nearest pose couldn't find for some reasons */ -TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( +bool calcNearestPoseInterp( const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, double * nearest_time, const double max_dist, const double max_yaw, const rclcpp::Logger & logger, @@ -177,11 +174,26 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( // /** // * @brief calculate distance to stopped point // */ -TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance( +double calcStopDistance( const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin); + +/** + * @brief extend terminal points + * Note: The current MPC does not properly take into account the attitude angle at the end of the + * path. By extending the end of the path in the attitude direction, the MPC can consider the + * attitude angle well, resulting in improved control performance. If the trajectory is + * well-defined considering the end point attitude angle, this feature is not necessary. + * @param [in] terminal yaw + * @param [in] extend interval + * @param [in] flag of forward shift + * @param [out] extended trajectory + */ +void extendTrajectoryInYawDirection( + const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); + } // namespace MPCUtils -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__MPC_UTILS_HPP_ +#endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index dc48253d7c0d0..27123abfa8beb 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -12,13 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" -#include "trajectory_follower/visibility_control.hpp" namespace autoware { @@ -26,11 +25,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /// Interface for solvers of Quadratic Programming (QP) problems -class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface +class QPSolverInterface { public: /** @@ -55,8 +54,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverInterface const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 546b16769bfc9..744ec33db2e57 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #include "eigen3/Eigen/Dense" +#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/qp_solver/qp_solver_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" namespace autoware { @@ -27,11 +26,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /// Solver for QP problems using the OSQP library -class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface +class QPSolverOSQP : public QPSolverInterface { public: /** @@ -65,8 +64,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverOSQP : public QPSolverInterface autoware::common::osqp::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp similarity index 81% rename from control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp index f96ff6621521c..82d88b15465ab 100644 --- a/control/trajectory_follower/include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ -#define TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Dense" #include "eigen3/Eigen/LU" -#include "trajectory_follower/qp_solver/qp_solver_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include @@ -29,14 +28,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Solver for QP problems using least square decomposition * implement with Eigen's standard Cholesky decomposition (LLT) */ -class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverInterface +class QPSolverEigenLeastSquareLLT : public QPSolverInterface { public: /** @@ -66,8 +65,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC QPSolverEigenLeastSquareLLT : public QPSolverIn const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp similarity index 88% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 91c873d54bad1..94a6326d3980a 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -44,13 +44,12 @@ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009. */ -#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -58,14 +57,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class of bicycle dynamics * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInterface +class DynamicsBicycleModel : public VehicleModelInterface { public: /** @@ -113,8 +112,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC DynamicsBicycleModel : public VehicleModelInter double m_cf; //!< @brief front cornering power [N/rad] double m_cr; //!< @brief rear cornering power [N/rad] }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 85% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index 6ab35f7a23413..dfd62a540a077 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -38,13 +38,12 @@ * */ -#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -52,14 +51,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class of bicycle kinematics * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInterface +class KinematicsBicycleModel : public VehicleModelInterface { public: /** @@ -97,8 +96,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModel : public VehicleModelInt double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp similarity index 82% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 88b3909bcf7f0..0306826344bbc 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -35,13 +35,12 @@ * */ -#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" -#include "trajectory_follower/visibility_control.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -49,14 +48,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class of bicycle kinematics without steering delay * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleModelInterface +class KinematicsBicycleModelNoDelay : public VehicleModelInterface { public: /** @@ -92,8 +91,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC KinematicsBicycleModelNoDelay : public VehicleM private: double m_steer_lim; //!< @brief steering angle limit [rad] }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp similarity index 88% rename from control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index b2ededcfb7808..86dc86941274e 100644 --- a/control/trajectory_follower/include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #include "eigen3/Eigen/Core" -#include "trajectory_follower/visibility_control.hpp" namespace autoware { @@ -24,14 +23,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { /** * Vehicle model class * @brief calculate model-related values */ -class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface +class VehicleModelInterface { protected: const int m_dim_x; //!< @brief dimension of state x @@ -110,8 +109,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC VehicleModelInterface */ virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; }; -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml new file mode 100644 index 0000000000000..c805abe341bef --- /dev/null +++ b/control/mpc_lateral_controller/package.xml @@ -0,0 +1,50 @@ + + + + mpc_lateral_controller + 1.0.0 + MPC-based lateral controller + + Takamasa Horibe + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_geometry + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + trajectory_follower_base + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml similarity index 97% rename from control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml rename to control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index a6d98fa682d74..92d15da9e49e6 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -12,6 +12,9 @@ curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + # -- trajectory extending -- + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) mpc_prediction_horizon: 50 # prediction horizon step diff --git a/control/trajectory_follower/src/interpolate.cpp b/control/mpc_lateral_controller/src/interpolate.cpp similarity index 97% rename from control/trajectory_follower/src/interpolate.cpp rename to control/mpc_lateral_controller/src/interpolate.cpp index 3e833dcd8b183..e3309fed61652 100644 --- a/control/trajectory_follower/src/interpolate.cpp +++ b/control/mpc_lateral_controller/src/interpolate.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/interpolate.hpp" +#include "mpc_lateral_controller/interpolate.hpp" #include #include @@ -28,7 +28,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace { @@ -137,7 +137,7 @@ bool linearInterpolate( return_value = return_value_v.at(0); return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp similarity index 97% rename from control/trajectory_follower/src/longitudinal_controller_utils.cpp rename to control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp index 21a79b574c93f..6f65171e2c80a 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/mpc_lateral_controller/src/longitudinal_controller_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/longitudinal_controller_utils.hpp" +#include "mpc_lateral_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" @@ -34,7 +34,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace longitudinal_utils { @@ -182,7 +182,7 @@ double applyDiffLimitFilter( return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); } } // namespace longitudinal_utils -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp similarity index 96% rename from control/trajectory_follower/src/lowpass_filter.cpp rename to control/mpc_lateral_controller/src/lowpass_filter.cpp index e45221cf9c6a5..40097e46cd1c4 100644 --- a/control/trajectory_follower/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/lowpass_filter.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz) { @@ -142,7 +142,7 @@ bool filt_vector(const int num, std::vector & u) return true; } } // namespace MoveAverageFilter -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp similarity index 89% rename from control/trajectory_follower/src/mpc.cpp rename to control/mpc_lateral_controller/src/mpc.cpp index 050727c40a50d..5a3d56319a7be 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc.hpp" +#include "mpc_lateral_controller/mpc.hpp" #include "motion_utils/motion_utils.hpp" @@ -33,7 +33,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { using namespace std::literals::chrono_literals; @@ -45,7 +45,7 @@ bool MPC::calculateMPC( tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic) { /* recalculate velocity from ego-velocity with dynamics */ - trajectory_follower::MPCTrajectory reference_trajectory = + mpc_lateral_controller::MPCTrajectory reference_trajectory = applyVelocityDynamicsFilter(m_ref_traj, current_pose, current_velocity); MPCData mpc_data; @@ -66,7 +66,7 @@ bool MPC::calculateMPC( } /* resample ref_traj with mpc sampling time */ - trajectory_follower::MPCTrajectory mpc_resampled_ref_traj; + mpc_lateral_controller::MPCTrajectory mpc_resampled_ref_traj; const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay; const double prediction_dt = getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_pose); @@ -107,7 +107,7 @@ bool MPC::calculateMPC( /* calculate predicted trajectory */ Eigen::VectorXd Xex = mpc_matrix.Aex * x0 + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - trajectory_follower::MPCTrajectory mpc_predicted_traj; + mpc_lateral_controller::MPCTrajectory mpc_predicted_traj; const auto & traj = mpc_resampled_ref_traj; for (size_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { const int DIM_X = m_vehicle_model_ptr->getDimX(); @@ -123,7 +123,7 @@ bool MPC::calculateMPC( const double relative_time = traj.relative_time[i]; mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); } - trajectory_follower::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); + mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); /* prepare diagnostic message */ const double nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; @@ -180,15 +180,15 @@ void MPC::setReferenceTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg, const double traj_resample_dist, const bool enable_path_smoothing, const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, - const int curvature_smoothing_num_ref_steer) + const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control) { - trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory - trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory - trajectory_follower::MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory + mpc_lateral_controller::MPCTrajectory mpc_traj_raw; // received raw trajectory + mpc_lateral_controller::MPCTrajectory mpc_traj_resampled; // resampled trajectory + mpc_lateral_controller::MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory /* resampling */ - trajectory_follower::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); - if (!trajectory_follower::MPCUtils::resampleMPCTrajectoryByDistance( + mpc_lateral_controller::MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); + if (!mpc_lateral_controller::MPCUtils::resampleMPCTrajectoryByDistance( mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) { RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance"); return; @@ -204,25 +204,36 @@ void MPC::setReferenceTrajectory( const int mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size()); if (enable_path_smoothing && mpc_traj_resampled_size > 2 * path_filter_moving_ave_num) { if ( - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.x) || - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.y) || - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || - !trajectory_follower::MoveAverageFilter::filt_vector( + !mpc_lateral_controller::MoveAverageFilter::filt_vector( path_filter_moving_ave_num, mpc_traj_smoothed.vx)) { RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering."); mpc_traj_smoothed = mpc_traj_resampled; } } + /* extend terminal points + * Note: The current MPC does not properly take into account the attitude angle at the end of the + * path. By extending the end of the path in the attitude direction, the MPC can consider the + * attitude angle well, resulting in improved control performance. If the trajectory is + * well-defined considering the end point attitude angle, this feature is not necessary. + */ + if (extend_trajectory_for_end_yaw_control) { + mpc_lateral_controller::MPCUtils::extendTrajectoryInYawDirection( + mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed); + } + /* calculate yaw angle */ - trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); - trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); + mpc_lateral_controller::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); + mpc_lateral_controller::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); /* calculate curvature */ - trajectory_follower::MPCUtils::calcTrajectoryCurvature( + mpc_lateral_controller::MPCUtils::calcTrajectoryCurvature( static_cast(curvature_smoothing_num_traj), static_cast(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed); @@ -253,13 +264,13 @@ void MPC::resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport } bool MPC::getData( - const trajectory_follower::MPCTrajectory & traj, + const mpc_lateral_controller::MPCTrajectory & traj, const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer, const geometry_msgs::msg::Pose & current_pose, MPCData * data) { static constexpr auto duration = 5000 /*ms*/; size_t nearest_idx; - if (!trajectory_follower::MPCUtils::calcNearestPoseInterp( + if (!mpc_lateral_controller::MPCUtils::calcNearestPoseInterp( traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) { // reset previous MPC result @@ -277,7 +288,7 @@ bool MPC::getData( data->nearest_idx = static_cast(nearest_idx); data->steer = static_cast(current_steer.steering_tire_angle); data->lateral_err = - trajectory_follower::MPCUtils::calcLateralError(current_pose, data->nearest_pose); + mpc_lateral_controller::MPCUtils::calcLateralError(current_pose, data->nearest_pose); data->yaw_err = autoware::common::helper_functions::wrap_angle( tf2::getYaw(current_pose.orientation) - tf2::getYaw(data->nearest_pose.orientation)); @@ -395,14 +406,14 @@ void MPC::storeSteerCmd(const double steer) } bool MPC::resampleMPCTrajectoryByTime( - const double ts, const double prediction_dt, const trajectory_follower::MPCTrajectory & input, - trajectory_follower::MPCTrajectory * output) const + const double ts, const double prediction_dt, const mpc_lateral_controller::MPCTrajectory & input, + mpc_lateral_controller::MPCTrajectory * output) const { std::vector mpc_time_v; for (double i = 0; i < static_cast(m_param.prediction_horizon); ++i) { mpc_time_v.push_back(ts + i * prediction_dt); } - if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory( + if (!mpc_lateral_controller::MPCUtils::linearInterpMPCTrajectory( input.relative_time, input, mpc_time_v, output)) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, 1000 /*ms*/, @@ -442,7 +453,8 @@ Eigen::VectorXd MPC::getInitialState(const MPCData & data) } bool MPC::updateStateForDelayCompensation( - const trajectory_follower::MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x) + const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time, + Eigen::VectorXd * x) { const int DIM_X = m_vehicle_model_ptr->getDimX(); const int DIM_U = m_vehicle_model_ptr->getDimU(); @@ -459,8 +471,8 @@ bool MPC::updateStateForDelayCompensation( double k = 0.0; double v = 0.0; if ( - !trajectory_follower::linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) || - !trajectory_follower::linearInterpolate(traj.relative_time, traj.vx, mpc_curr_time, v)) { + !mpc_lateral_controller::linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) || + !mpc_lateral_controller::linearInterpolate(traj.relative_time, traj.vx, mpc_curr_time, v)) { RCLCPP_ERROR( m_logger, "mpc resample error at delay compensation, stop mpc calculation. check code!"); return false; @@ -479,12 +491,12 @@ bool MPC::updateStateForDelayCompensation( return true; } -trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( - const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose, - const double v0) const +mpc_lateral_controller::MPCTrajectory MPC::applyVelocityDynamicsFilter( + const mpc_lateral_controller::MPCTrajectory & input, + const geometry_msgs::msg::Pose & current_pose, const double v0) const { autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( input, autoware_traj); if (autoware_traj.points.empty()) { return input; @@ -496,8 +508,8 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( const double acc_lim = m_param.acceleration_limit; const double tau = m_param.velocity_time_constant; - trajectory_follower::MPCTrajectory output = input; - trajectory_follower::MPCUtils::dynamicSmoothingVelocity( + mpc_lateral_controller::MPCTrajectory output = input; + mpc_lateral_controller::MPCUtils::dynamicSmoothingVelocity( static_cast(nearest_idx), v0, acc_lim, tau, output); const double t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time const double t_end = output.relative_time.back() + t_ext; @@ -515,7 +527,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ MPCMatrix MPC::generateMPCMatrix( - const trajectory_follower::MPCTrajectory & reference_trajectory, const double prediction_dt) + const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt) { using Eigen::MatrixXd; @@ -777,12 +789,12 @@ void MPC::addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f_ptr) c } double MPC::getPredictionDeltaTime( - const double start_time, const trajectory_follower::MPCTrajectory & input, + const double start_time, const mpc_lateral_controller::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose) const { // Calculate the time min_prediction_length ahead from current_pose autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( input, autoware_traj); const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); @@ -854,7 +866,7 @@ bool MPC::isValid(const MPCMatrix & m) const return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp similarity index 85% rename from control/trajectory_follower/src/mpc_lateral_controller.cpp rename to control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index aaefa696d6494..9ccb35bac795d 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc_lateral_controller.hpp" +#include "mpc_lateral_controller/mpc_lateral_controller.hpp" #include "motion_utils/motion_utils.hpp" #include "tf2_ros/create_timer_ros.h" @@ -31,7 +31,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace { @@ -65,6 +65,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter("admissible_yaw_error_rad"); m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); m_mpc.m_param.steer_tau = node_->declare_parameter("vehicle_model_steer_tau"); + m_extend_trajectory_for_end_yaw_control = + node_->declare_parameter("extend_trajectory_for_end_yaw_control"); /* stop state parameters */ m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); @@ -87,12 +89,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* vehicle model setup */ const std::string vehicle_model_type = node_->declare_parameter("vehicle_model_type"); - std::shared_ptr vehicle_model_ptr; + std::shared_ptr vehicle_model_ptr; if (vehicle_model_type == "kinematics") { - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); } else if (vehicle_model_type == "kinematics_no_delay") { - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, m_mpc.m_steer_lim); } else if (vehicle_model_type == "dynamics") { const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); @@ -104,7 +106,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time // // NOLINT - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); } else { RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); @@ -112,11 +114,11 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* QP solver setup */ const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); - std::shared_ptr qpsolver_ptr; + std::shared_ptr qpsolver_ptr; if (qp_solver_type == "unconstraint_fast") { - qpsolver_ptr = std::make_shared(); + qpsolver_ptr = std::make_shared(); } else if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(node_->get_logger()); + qpsolver_ptr = std::make_shared(node_->get_logger()); } else { RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); } @@ -171,11 +173,13 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} MpcLateralController::~MpcLateralController() {} -boost::optional MpcLateralController::run() +trajectory_follower::LateralOutput MpcLateralController::run( + trajectory_follower::InputData const & input_data) { - if (!checkData()) { - return boost::none; - } + // set input data + setTrajectory(input_data.current_trajectory); + m_current_kinematic_state = input_data.current_odometry; + m_current_steering = input_data.current_steering; autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; autoware_auto_planning_msgs::msg::Trajectory predicted_traj; @@ -187,17 +191,17 @@ boost::optional MpcLateralController::run() } const bool is_mpc_solved = m_mpc.calculateMPC( - *m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x, - m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, debug_values); + m_current_steering, m_current_kinematic_state.twist.twist.linear.x, + m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); publishPredictedTraj(predicted_traj); publishDebugValues(debug_values); const auto createLateralOutput = [this](const auto & cmd) { - LateralOutput output; + trajectory_follower::LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); output.sync_data.is_steer_converged = isSteerConverged(cmd); - return boost::optional(output); + return output; }; if (isStoppedState()) { @@ -221,13 +225,6 @@ boost::optional MpcLateralController::run() return createLateralOutput(ctrl_cmd); } -void MpcLateralController::setInputData(InputData const & input_data) -{ - setTrajectory(input_data.current_trajectory_ptr); - m_current_kinematic_state_ptr = input_data.current_odometry_ptr; - m_current_steering_ptr = input_data.current_steering_ptr; -} - bool MpcLateralController::isSteerConverged( const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const { @@ -239,73 +236,58 @@ bool MpcLateralController::isSteerConverged( } const bool is_converged = - std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle) < static_cast(m_converged_steer_rad); return is_converged; } -bool MpcLateralController::checkData() const +bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data) { + setTrajectory(input_data.current_trajectory); + m_current_kinematic_state = input_data.current_odometry; + m_current_steering = input_data.current_steering; + if (!m_mpc.hasVehicleModel()) { - RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model"); + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a vehicle model"); return false; } if (!m_mpc.hasQPSolver()) { - RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a QP solver"); - return false; - } - - if (!m_current_kinematic_state_ptr) { - RCLCPP_DEBUG( - node_->get_logger(), "waiting data. kinematic_state = %d", - m_current_kinematic_state_ptr != nullptr); + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a QP solver"); return false; } - - if (!m_current_steering_ptr) { - RCLCPP_DEBUG( - node_->get_logger(), "waiting data. current_steering = %d", - m_current_steering_ptr != nullptr); - return false; - } - if (m_mpc.m_ref_traj.size() == 0) { - RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero."); + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "trajectory size is zero."); return false; } return true; } -void MpcLateralController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg) { - if (!msg) return; - - m_current_trajectory_ptr = msg; - - if (!m_current_kinematic_state_ptr) { - RCLCPP_DEBUG(node_->get_logger(), "Current kinematic state is not received yet."); - return; - } + m_current_trajectory = msg; - if (msg->points.size() < 3) { + if (msg.points.size() < 3) { RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough."); return; } - if (!isValidTrajectory(*msg)) { + if (!isValidTrajectory(msg)) { RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing."); return; } m_mpc.setReferenceTrajectory( - *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, - m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer); + msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, + m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, + m_extend_trajectory_for_end_yaw_control); // update trajectory buffer to check the trajectory shape change. - m_trajectory_buffer.push_back(*m_current_trajectory_ptr); + m_trajectory_buffer.push_back(m_current_trajectory); while (rclcpp::ok()) { const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) - rclcpp::Time(m_trajectory_buffer.front().header.stamp); @@ -334,7 +316,7 @@ autoware_auto_control_msgs::msg::AckermannLateralCommand MpcLateralController::getInitialControlCommand() const { autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; - cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle; + cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; } @@ -342,7 +324,7 @@ MpcLateralController::getInitialControlCommand() const bool MpcLateralController::isStoppedState() const { // If the nearest index is not found, return false - if (m_current_trajectory_ptr->points.empty()) { + if (m_current_trajectory.points.empty()) { return false; } @@ -351,12 +333,12 @@ bool MpcLateralController::isStoppedState() const // control was turned off when approaching/exceeding the stop line on a curve or // emergency stop situation and it caused large tracking error. const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_current_trajectory_ptr->points, m_current_kinematic_state_ptr->pose.pose, - m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); - const double current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; + const double current_vel = m_current_kinematic_state.twist.twist.linear.x; const double target_vel = - m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; + m_current_trajectory.points.at(static_cast(nearest)).longitudinal_velocity_mps; const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { @@ -384,7 +366,7 @@ void MpcLateralController::publishPredictedTraj( autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const { predicted_traj.header.stamp = node_->now(); - predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id; + predicted_traj.header.frame_id = m_current_trajectory.header.frame_id; m_pub_predicted_traj->publish(predicted_traj); } @@ -448,7 +430,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - trajectory_follower::MPCParam param = m_mpc.m_param; + mpc_lateral_controller::MPCParam param = m_mpc.m_param; try { update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); update_param(parameters, "mpc_prediction_dt", param.prediction_dt); @@ -518,7 +500,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const for (const auto & trajectory : m_trajectory_buffer) { if ( tier4_autoware_utils::calcDistance2d( - trajectory.points.back().pose, m_current_trajectory_ptr->points.back().pose) > + trajectory.points.back().pose, m_current_trajectory.points.back().pose) > m_new_traj_end_dist) { return true; } @@ -543,7 +525,7 @@ bool MpcLateralController::isValidTrajectory( return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp similarity index 93% rename from control/trajectory_follower/src/mpc_trajectory.cpp rename to control/mpc_lateral_controller/src/mpc_trajectory.cpp index cf0a45763a7ff..810d7eaf7997f 100644 --- a/control/trajectory_follower/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" namespace autoware { @@ -20,7 +20,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { void MPCTrajectory::push_back( const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp, @@ -60,7 +60,7 @@ size_t MPCTrajectory::size() const return 0; } } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp similarity index 91% rename from control/trajectory_follower/src/mpc_utils.cpp rename to control/mpc_lateral_controller/src/mpc_utils.cpp index e4270909cdf21..142dfc50f0751 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/mpc_utils.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" #include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -28,7 +28,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { namespace MPCUtils { @@ -265,10 +265,9 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj) traj.relative_time.clear(); traj.relative_time.push_back(t); for (size_t i = 0; i < traj.x.size() - 1; ++i) { - const double dx = traj.x.at(i + 1) - traj.x.at(i); - const double dy = traj.y.at(i + 1) - traj.y.at(i); - const double dz = traj.z.at(i + 1) - traj.z.at(i); - const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + const double dist = std::hypot( + traj.x.at(i + 1) - traj.x.at(i), traj.y.at(i + 1) - traj.y.at(i), + traj.z.at(i + 1) - traj.z.at(i)); const double v = std::max(std::fabs(traj.vx.at(i)), 0.1); t += (dist / v); traj.relative_time.push_back(t); @@ -404,8 +403,33 @@ double calcStopDistance( return stop_dist; } +void extendTrajectoryInYawDirection( + const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj) +{ + // set terminal yaw + traj.yaw.back() = yaw; + + // get terminal pose + autoware_auto_planning_msgs::msg::Trajectory autoware_traj; + autoware::motion::control::mpc_lateral_controller::MPCUtils::convertToAutowareTrajectory( + traj, autoware_traj); + auto extended_pose = autoware_traj.points.back().pose; + + constexpr double extend_dist = 10.0; + constexpr double extend_vel = 10.0; + const double x_offset = is_forward_shift ? interval : -interval; + const double dt = interval / extend_vel; + const size_t num_extended_point = static_cast(extend_dist / interval); + for (size_t i = 0; i < num_extended_point; ++i) { + extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0); + traj.push_back( + extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(), + extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt); + } +} + } // namespace MPCUtils -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp similarity index 95% rename from control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index cbfd57e998503..a90c4457e36a5 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include #include @@ -23,7 +23,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { QPSolverOSQP::QPSolverOSQP(const rclcpp::Logger & logger) : logger_{logger} {} bool QPSolverOSQP::solve( @@ -80,7 +80,7 @@ bool QPSolverOSQP::solve( } return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp similarity index 89% rename from control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp index 18692d175922b..7208473ccef6d 100644 --- a/control/trajectory_follower/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" namespace autoware { @@ -20,7 +20,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() {} bool QPSolverEigenLeastSquareLLT::solve( @@ -36,7 +36,7 @@ bool QPSolverEigenLeastSquareLLT::solve( return true; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/smooth_stop.cpp b/control/mpc_lateral_controller/src/smooth_stop.cpp similarity index 97% rename from control/trajectory_follower/src/smooth_stop.cpp rename to control/mpc_lateral_controller/src/smooth_stop.cpp index 9b5dc14f75248..26cfcabfebead 100644 --- a/control/trajectory_follower/src/smooth_stop.cpp +++ b/control/mpc_lateral_controller/src/smooth_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/smooth_stop.hpp" +#include "mpc_lateral_controller/smooth_stop.hpp" #include // NOLINT @@ -28,7 +28,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { void SmoothStop::init(const double pred_vel_in_target, const double pred_stop_dist) { @@ -164,7 +164,7 @@ double SmoothStop::calculate( // when the car is not running return m_params.strong_stop_acc; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp similarity index 95% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index fa7e354b6d318..a634fa02232b4 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { DynamicsBicycleModel::DynamicsBicycleModel( const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl, @@ -92,7 +92,7 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 94% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 73c3aabea3fb0..9955ce43fcb1b 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { KinematicsBicycleModel::KinematicsBicycleModel( const double wheelbase, const double steer_lim, const double steer_tau) @@ -74,7 +74,7 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp similarity index 92% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index d9a399c5740c2..7cd94e1406e10 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include @@ -22,7 +22,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay( const double wheelbase, const double steer_lim) @@ -65,7 +65,7 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp similarity index 90% rename from control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp rename to control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 52a0d5c7f9c8b..432e98a672e88 100644 --- a/control/trajectory_follower/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/vehicle_model/vehicle_model_interface.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware { @@ -20,7 +20,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace mpc_lateral_controller { VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase) : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) @@ -32,7 +32,7 @@ int VehicleModelInterface::getDimY() { return m_dim_y; } double VehicleModelInterface::getWheelbase() { return m_wheelbase; } void VehicleModelInterface::setVelocity(const double velocity) { m_velocity = velocity; } void VehicleModelInterface::setCurvature(const double curvature) { m_curvature = curvature; } -} // namespace trajectory_follower +} // namespace mpc_lateral_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/test/test_interpolate.cpp b/control/mpc_lateral_controller/test/test_interpolate.cpp similarity index 93% rename from control/trajectory_follower/test/test_interpolate.cpp rename to control/mpc_lateral_controller/test/test_interpolate.cpp index 69715b024d0dd..97b2d28205ee4 100644 --- a/control/trajectory_follower/test/test_interpolate.cpp +++ b/control/mpc_lateral_controller/test/test_interpolate.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/interpolate.hpp" +#include "mpc_lateral_controller/interpolate.hpp" #include TEST(TestInterpolate, Nominal) { - using autoware::motion::control::trajectory_follower::linearInterpolate; + using autoware::motion::control::mpc_lateral_controller::linearInterpolate; // Simple case { @@ -62,7 +62,7 @@ TEST(TestInterpolate, Nominal) } TEST(TestInterpolate, Failure) { - using autoware::motion::control::trajectory_follower::linearInterpolate; + using autoware::motion::control::mpc_lateral_controller::linearInterpolate; std::vector target_values; diff --git a/control/trajectory_follower/test/test_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp similarity index 74% rename from control/trajectory_follower/test/test_lowpass_filter.cpp rename to control/mpc_lateral_controller/test/test_lowpass_filter.cpp index 691cdebb9b94d..89afe436dd2d1 100644 --- a/control/trajectory_follower/test/test_lowpass_filter.cpp +++ b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -13,41 +13,20 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/lowpass_filter.hpp" +#include "mpc_lateral_controller/lowpass_filter.hpp" #include -TEST(TestLowpassFilter, LowpassFilter1d) -{ - using autoware::motion::control::trajectory_follower::LowpassFilter1d; - - const double epsilon = 1e-6; - LowpassFilter1d lowpass_filter_1d(0.0, 0.1); - - // initial state - EXPECT_NEAR(lowpass_filter_1d.getValue(), 0.0, epsilon); - - // random filter - EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); - EXPECT_NEAR(lowpass_filter_1d.filter(1.0), 0.9, epsilon); - EXPECT_NEAR(lowpass_filter_1d.filter(2.0), 1.89, epsilon); - EXPECT_NEAR(lowpass_filter_1d.getValue(), 1.89, epsilon); - - // reset - lowpass_filter_1d.reset(-1.1); - EXPECT_NEAR(lowpass_filter_1d.getValue(), -1.1, epsilon); - EXPECT_NEAR(lowpass_filter_1d.filter(0.0), -0.11, epsilon); - EXPECT_NEAR(lowpass_filter_1d.getValue(), -0.11, epsilon); -} TEST(TestLowpassFilter, MoveAverageFilter) { - namespace MoveAverageFilter = autoware::motion::control::trajectory_follower::MoveAverageFilter; + namespace MoveAverageFilter = + autoware::motion::control::mpc_lateral_controller::MoveAverageFilter; { // Fail case: window size higher than the vector size const int window_size = 5; std::vector vec = {1.0, 2.0, 3.0, 4.0}; EXPECT_FALSE(MoveAverageFilter::filt_vector(window_size, vec)); - } // namespace autoware::motion::control::trajectory_follower::MoveAverageFilter; + } // namespace autoware::motion::control::mpc_lateral_controller::MoveAverageFilter; { const int window_size = 0; const std::vector original_vec = {1.0, 3.0, 4.0, 6.0}; @@ -85,7 +64,7 @@ TEST(TestLowpassFilter, MoveAverageFilter) } TEST(TestLowpassFilter, Butterworth2dFilter) { - using autoware::motion::control::trajectory_follower::Butterworth2dFilter; + using autoware::motion::control::mpc_lateral_controller::Butterworth2dFilter; const double dt = 1.0; const double cutoff_hz = 1.0; Butterworth2dFilter filter(dt, cutoff_hz); diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp similarity index 78% rename from control/trajectory_follower/test/test_mpc.cpp rename to control/mpc_lateral_controller/test/test_mpc.cpp index 909133dcd65bd..056dcd77c2154 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -13,10 +13,10 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/mpc.hpp" -#include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" -#include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" -#include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "mpc_lateral_controller/mpc.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -38,7 +38,7 @@ namespace { -namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +namespace mpc_lateral_controller = ::autoware::motion::control::mpc_lateral_controller; typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; typedef autoware_auto_vehicle_msgs::msg::SteeringReport SteeringReport; @@ -50,7 +50,7 @@ typedef tier4_debug_msgs::msg::Float32MultiArrayStamped Float32MultiArrayStamped class MPCTest : public ::testing::Test { protected: - trajectory_follower::MPCParam param; + mpc_lateral_controller::MPCParam param; // Test inputs Trajectory dummy_straight_trajectory; Trajectory dummy_right_turn_trajectory; @@ -84,6 +84,7 @@ class MPCTest : public ::testing::Test int curvature_smoothing_num_ref_steer = 35; bool enable_path_smoothing = true; bool use_steer_prediction = true; + bool extend_trajectory_for_end_yaw_control = true; void SetUp() override { @@ -161,7 +162,7 @@ class MPCTest : public ::testing::Test pose_zero_ptr->pose = pose_zero; } - void initializeMPC(trajectory_follower::MPC & mpc) + void initializeMPC(mpc_lateral_controller::MPC & mpc) { mpc.m_param = param; mpc.m_admissible_position_error = admissible_position_error; @@ -175,26 +176,27 @@ class MPCTest : public ::testing::Test // Init trajectory mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); } }; // class MPCTest /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; EXPECT_FALSE(mpc.hasVehicleModel()); EXPECT_FALSE(mpc.hasQPSolver()); const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -213,19 +215,19 @@ TEST_F(MPCTest, InitializeAndCalculate) TEST_F(MPCTest, InitializeAndCalculateRightTurn) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; EXPECT_FALSE(mpc.hasVehicleModel()); EXPECT_FALSE(mpc.hasQPSolver()); const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -233,7 +235,8 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); // Calculate MPC AckermannLateralCommand ctrl_cmd; @@ -247,21 +250,22 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) TEST_F(MPCTest, OsqpCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(logger); + std::shared_ptr qpsolver_ptr = + std::make_shared(logger); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -278,21 +282,22 @@ TEST_F(MPCTest, OsqpCalculate) TEST_F(MPCTest, OsqpCalculateRightTurn) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(logger); + std::shared_ptr qpsolver_ptr = + std::make_shared(logger); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -308,17 +313,17 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) TEST_F(MPCTest, KinematicsNoDelayCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); const std::string vehicle_model_type = "kinematics_no_delay"; - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -327,7 +332,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init trajectory mpc.setReferenceTrajectory( dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; @@ -340,20 +346,21 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); mpc.setReferenceTrajectory( dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer); + path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, + extend_trajectory_for_end_yaw_control); const std::string vehicle_model_type = "kinematics_no_delay"; - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -372,18 +379,18 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) TEST_F(MPCTest, DynamicCalculate) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; initializeMPC(mpc); const std::string vehicle_model_type = "dynamics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -399,14 +406,14 @@ TEST_F(MPCTest, DynamicCalculate) TEST_F(MPCTest, MultiSolveWithBuffer) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory @@ -441,14 +448,14 @@ TEST_F(MPCTest, MultiSolveWithBuffer) TEST_F(MPCTest, FailureCases) { - trajectory_follower::MPC mpc; + mpc_lateral_controller::MPC mpc; const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( + std::shared_ptr vehicle_model_ptr = + std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = + std::make_shared(); mpc.setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory @@ -470,7 +477,7 @@ TEST_F(MPCTest, FailureCases) // Set a wrong vehicle model (not a failure but generates an error message) const std::string wrong_vehicle_model_type = "wrong_model"; - vehicle_model_ptr = std::make_shared( + vehicle_model_ptr = std::make_shared( wheelbase, steer_limit, steer_tau); mpc.setVehicleModel(vehicle_model_ptr, wrong_vehicle_model_type); EXPECT_TRUE( diff --git a/control/trajectory_follower/test/test_mpc_utils.cpp b/control/mpc_lateral_controller/test/test_mpc_utils.cpp similarity index 94% rename from control/trajectory_follower/test/test_mpc_utils.cpp rename to control/mpc_lateral_controller/test/test_mpc_utils.cpp index 7883537885bec..01c8e55699c62 100644 --- a/control/trajectory_follower/test/test_mpc_utils.cpp +++ b/control/mpc_lateral_controller/test/test_mpc_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/mpc_trajectory.hpp" -#include "trajectory_follower/mpc_utils.hpp" +#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -25,7 +25,7 @@ namespace { -namespace MPCUtils = ::autoware::motion::control::trajectory_follower::MPCUtils; +namespace MPCUtils = ::autoware::motion::control::mpc_lateral_controller::MPCUtils; typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; typedef geometry_msgs::msg::Pose Pose; diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index a0ddef6e363cc..a86443f5cabdb 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -3,6 +3,8 @@ transition_timeout: 10.0 frequency_hz: 10.0 check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 775083908cdfd..c823c11a4f4ba 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -14,6 +14,8 @@ #include "state.hpp" +#include "util.hpp" + #include #include @@ -43,6 +45,10 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + nearest_dist_deviation_threshold_ = + node->declare_parameter("nearest_dist_deviation_threshold"); + nearest_yaw_deviation_threshold_ = + node->declare_parameter("nearest_yaw_deviation_threshold"); // params for mode change available { @@ -86,9 +92,6 @@ bool AutonomousMode::isModeChangeCompleted() return true; } - constexpr auto dist_max = 5.0; - constexpr auto yaw_max = M_PI_4; - const auto current_speed = kinematics_.twist.twist.linear.x; const auto & param = engage_acceptable_param_; @@ -107,8 +110,9 @@ bool AutonomousMode::isModeChangeCompleted() return unstable(); } - const auto closest_idx = - findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + const auto closest_idx = findNearestIndex( + trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (!closest_idx) { RCLCPP_INFO(logger_, "Not stable yet: closest point not found"); return unstable(); @@ -195,13 +199,10 @@ std::pair AutonomousMode::hasDangerLateralAcceleration() bool AutonomousMode::isModeChangeAvailable() { if (!check_engage_condition_) { - debug_info_.is_all_ok = true; + setAllOk(debug_info_); return true; } - constexpr auto dist_max = 100.0; - constexpr auto yaw_max = M_PI_4; - const auto current_speed = kinematics_.twist.twist.linear.x; const auto target_control_speed = control_cmd_.longitudinal.speed; const auto & param = engage_acceptable_param_; @@ -213,8 +214,9 @@ bool AutonomousMode::isModeChangeAvailable() return false; } - const auto closest_idx = - findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + const auto closest_idx = findNearestIndex( + trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (!closest_idx) { RCLCPP_INFO(logger_, "Engage unavailable: closest point not found"); debug_info_ = DebugInfo{}; // all false diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index dc72d57aed231..9d384857bbe3d 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -72,7 +72,9 @@ class AutonomousMode : public ModeChangeBase rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + double nearest_dist_deviation_threshold_; // [m] for finding nearest index + double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_; diff --git a/control/operation_mode_transition_manager/src/util.hpp b/control/operation_mode_transition_manager/src/util.hpp new file mode 100644 index 0000000000000..e3fa20aaafd74 --- /dev/null +++ b/control/operation_mode_transition_manager/src/util.hpp @@ -0,0 +1,37 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTIL_HPP_ +#define UTIL_HPP_ + +#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" + +using DebugInfo = operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; + +void setAllOk(DebugInfo & debug_info) +{ + debug_info.is_all_ok = true; + debug_info.engage_allowed_for_stopped_vehicle = true; + debug_info.large_acceleration_ok = true; + debug_info.large_lateral_acceleration_diff_ok = true; + debug_info.large_lateral_acceleration_ok = true; + debug_info.lateral_deviation_ok = true; + debug_info.speed_lower_deviation_ok = true; + debug_info.speed_upper_deviation_ok = true; + debug_info.stop_ok = true; + debug_info.trajectory_available_ok = true; + debug_info.yaw_deviation_ok = true; +} + +#endif // UTIL_HPP_ diff --git a/control/pid_longitudinal_controller/CMakeLists.txt b/control/pid_longitudinal_controller/CMakeLists.txt new file mode 100644 index 0000000000000..3e7a992b15e00 --- /dev/null +++ b/control/pid_longitudinal_controller/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(pid_longitudinal_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${PID_LON_CON_LIB} SHARED + src/pid_longitudinal_controller.cpp + src/pid.cpp + src/smooth_stop.cpp + src/longitudinal_controller_utils.cpp +) + +if(BUILD_TESTING) + set(TEST_LON_SOURCES + test/test_pid.cpp + test/test_smooth_stop.cpp + test/test_longitudinal_controller_utils.cpp + ) + set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) + ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) + target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${PID_LON_CON_LIB}) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param +) diff --git a/control/trajectory_follower/design/pid_longitudinal_controller-design.md b/control/pid_longitudinal_controller/README.md similarity index 98% rename from control/trajectory_follower/design/pid_longitudinal_controller-design.md rename to control/pid_longitudinal_controller/README.md index 0cca6803490cd..7147a76b8eeff 100644 --- a/control/trajectory_follower/design/pid_longitudinal_controller-design.md +++ b/control/pid_longitudinal_controller/README.md @@ -79,7 +79,7 @@ This PID logic has a maximum value for the output of each term. This is to preve - Large integral terms may cause unintended behavior by users. - Unintended noise may cause the output of the derivative term to be very large. -Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as "Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. +Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers. At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. @@ -125,14 +125,14 @@ There are two sources of the slope information, which can be switched by a param ### Input -Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Set the following from the [controller_node](../trajectory_follower_node/README.md) - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry ### Output -Return LongitudinalOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) +Return LongitudinalOutput which contains the following to the controller node - `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. - LongitudinalSyncData diff --git a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp similarity index 89% rename from control/trajectory_follower/include/trajectory_follower/debug_values.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp index b8c2ac445568f..2f20f28dd6da9 100644 --- a/control/trajectory_follower/include/trajectory_follower/debug_values.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp @@ -12,10 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ -#define TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ - -#include "trajectory_follower/visibility_control.hpp" +#ifndef PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ #include @@ -25,11 +23,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { /// Debug Values used for debugging or controller tuning -class TRAJECTORY_FOLLOWER_PUBLIC DebugValues +class DebugValues { public: /// Types of debug values @@ -96,9 +94,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues private: std::array(TYPE::SIZE)> m_values; }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp similarity index 83% rename from control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 084da2e6f7198..32928d64a12b8 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#ifndef PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -21,7 +21,6 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -37,7 +36,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { namespace longitudinal_utils { @@ -51,18 +50,18 @@ using geometry_msgs::msg::Quaternion; /** * @brief check if trajectory is invalid or not */ -TRAJECTORY_FOLLOWER_PUBLIC bool isValidTrajectory(const Trajectory & traj); +bool isValidTrajectory(const Trajectory & traj); /** * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ -TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance( +double calcStopDistance( const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw); /** * @brief calculate pitch angle from estimated current pose */ -TRAJECTORY_FOLLOWER_PUBLIC double getPitchByPose(const Quaternion & quaternion); +double getPitchByPose(const Quaternion & quaternion); /** * @brief calculate pitch angle from trajectory on map @@ -71,20 +70,19 @@ TRAJECTORY_FOLLOWER_PUBLIC double getPitchByPose(const Quaternion & quaternion); * @param [in] closest_idx nearest index to current vehicle position * @param [in] wheel_base length of wheel base */ -TRAJECTORY_FOLLOWER_PUBLIC double getPitchByTraj( +double getPitchByTraj( const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); /** * @brief calculate elevation angle */ -TRAJECTORY_FOLLOWER_PUBLIC double calcElevationAngle( - const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); +double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); /** * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for * delayed time */ -TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay( +Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel); /** @@ -93,8 +91,7 @@ TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay( * @param [in] o_to second orientation * @param [in] ratio ratio between o_from and o_to for interpolation */ -TRAJECTORY_FOLLOWER_PUBLIC Quaternion -lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point @@ -102,7 +99,7 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double * @param [in] point Interpolated point is nearest to this point. */ template -TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( +TrajectoryPoint lerpTrajectoryPoint( const T & points, const Pose & pose, const double max_dist, const double max_yaw) { TrajectoryPoint interpolated_point; @@ -144,7 +141,7 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( * @param [in] dt time between current and previous one * @param [in] lim_val limitation value for differential */ -TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( +double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double lim_val); /** @@ -155,13 +152,13 @@ TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( * @param [in] max_val maximum value for differential * @param [in] min_val minimum value for differential */ -TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter( +double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val); } // namespace longitudinal_utils -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp new file mode 100644 index 0000000000000..6215ee7a15826 --- /dev/null +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp @@ -0,0 +1,75 @@ +// Copyright 2018-2021 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace pid_longitudinal_controller +{ + +/** + * @brief Simple filter with gain on the first derivative of the value + */ +class LowpassFilter1d +{ +private: + double m_x; //!< @brief current filtered value + double m_gain; //!< @brief gain value of first-order low-pass filter + +public: + /** + * @brief constructor with initial value and first-order gain + * @param [in] x initial value + * @param [in] gain first-order gain + */ + LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {} + + /** + * @brief set the current value of the filter + * @param [in] x new value + */ + void reset(const double x) { m_x = x; } + + /** + * @brief get the current value of the filter + */ + double getValue() const { return m_x; } + + /** + * @brief filter a new value + * @param [in] u new value + */ + double filter(const double u) + { + const double ret = m_gain * m_x + (1.0 - m_gain) * u; + m_x = ret; + return ret; + } +}; +} // namespace pid_longitudinal_controller +} // namespace control +} // namespace motion +} // namespace autoware +#endif // PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp similarity index 91% rename from control/trajectory_follower/include/trajectory_follower/pid.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp index d9392ba3ced91..b261191075e2c 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp @@ -12,10 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__PID_HPP_ -#define TRAJECTORY_FOLLOWER__PID_HPP_ - -#include "trajectory_follower/visibility_control.hpp" +#ifndef PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__PID_HPP_ #include @@ -25,11 +23,11 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { /// @brief implementation of a PID controller -class TRAJECTORY_FOLLOWER_PUBLIC PIDController +class PIDController { public: PIDController(); @@ -97,9 +95,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PIDController bool m_is_gains_set; bool m_is_limits_set; }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__PID_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp similarity index 87% rename from control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index f91c09ecf3527..ce4aa88bc7a10 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ -#define TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#ifndef PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ #include "diagnostic_updater/diagnostic_updater.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" +#include "pid_longitudinal_controller/debug_values.hpp" +#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "pid_longitudinal_controller/lowpass_filter.hpp" +#include "pid_longitudinal_controller/pid.hpp" +#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/debug_values.hpp" -#include "trajectory_follower/longitudinal_controller_base.hpp" -#include "trajectory_follower/longitudinal_controller_utils.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/pid.hpp" -#include "trajectory_follower/smooth_stop.hpp" +#include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" @@ -50,14 +50,14 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \class PidLongitudinalController /// \brief The node class used for generating longitudinal control commands (velocity/acceleration) -class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public LongitudinalControllerBase +class PidLongitudinalController : public trajectory_follower::LongitudinalControllerBase { public: explicit PidLongitudinalController(rclcpp::Node & node); @@ -91,9 +91,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal const std::vector & parameters); // pointers for ros topic - nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr}; - geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr m_current_accel_ptr{nullptr}; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr}; + nav_msgs::msg::Odometry m_current_kinematic_state; + geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel; + autoware_auto_planning_msgs::msg::Trajectory m_trajectory; // vehicle info double m_wheel_base; @@ -143,14 +143,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal StateTransitionParams m_state_transition_params; // drive - trajectory_follower::PIDController m_pid_vel; - std::shared_ptr m_lpf_vel_error{nullptr}; + PIDController m_pid_vel; + std::shared_ptr m_lpf_vel_error{nullptr}; double m_current_vel_threshold_pid_integrate; bool m_enable_brake_keeping_before_stop; double m_brake_keeping_acc; // smooth stop - trajectory_follower::SmoothStop m_smooth_stop; + SmoothStop m_smooth_stop; // stop struct StoppedStateParams @@ -180,7 +180,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // slope compensation bool m_use_traj_for_pitch; - std::shared_ptr m_lpf_pitch{nullptr}; + std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; @@ -203,7 +203,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal std::vector> m_vel_hist; // debug values - trajectory_follower::DebugValues m_debug_values; + DebugValues m_debug_values; std::shared_ptr m_last_running_time{std::make_shared(node_->now())}; @@ -223,30 +223,27 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal * @brief set current and previous velocity with received message * @param [in] msg current state message */ - void setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void setKinematicState(const nav_msgs::msg::Odometry & msg); /** * @brief set current acceleration with received message * @param [in] msg trajectory message */ - void setCurrentAcceleration( - const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void setCurrentAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped & msg); /** * @brief set reference trajectory with received message * @param [in] msg trajectory message */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); - /** - * @brief compute control command, and publish periodically - */ - boost::optional run() override; + bool isReady(const trajectory_follower::InputData & input_data) override; /** - * @brief set input data like current odometry and trajectory. + * @brief compute control command, and publish periodically */ - void setInputData(InputData const & input_data) override; + trajectory_follower::LongitudinalOutput run( + trajectory_follower::InputData const & input_data) override; /** * @brief calculate data for controllers whose type is ControlData @@ -382,9 +379,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp similarity index 93% rename from control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp rename to control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp index c86f319f1288c..45c042dc026be 100644 --- a/control/trajectory_follower/include/trajectory_follower/smooth_stop.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ -#define TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ +#ifndef PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#define PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -32,13 +31,13 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { /** * @brief Smooth stop class to implement vehicle specific deceleration profiles */ -class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop +class SmoothStop { public: /** @@ -117,9 +116,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC SmoothStop rclcpp::Time m_weak_acc_time; bool m_is_set_params = false; }; -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__SMOOTH_STOP_HPP_ +#endif // PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/trajectory_follower/design/media/BrakeKeeping.drawio.svg b/control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/trajectory_follower/design/media/BrakeKeeping.drawio.svg rename to control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg diff --git a/control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg b/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg rename to control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg rename to control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml new file mode 100644 index 0000000000000..e3ff39b187d14 --- /dev/null +++ b/control/pid_longitudinal_controller/package.xml @@ -0,0 +1,49 @@ + + + + pid_longitudinal_controller + 1.0.0 + PID-based longitudinal controller + + Takamasa Horibe + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_geometry + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + trajectory_follower_base + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml rename to control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp new file mode 100644 index 0000000000000..a056acc901d8d --- /dev/null +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -0,0 +1,188 @@ +// Copyright 2018-2021 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 "pid_longitudinal_controller/longitudinal_controller_utils.hpp" + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + +#include // NOLINT + +#ifdef ROS_DISTRO_GALACTIC +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#else +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#endif + +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace pid_longitudinal_controller +{ +namespace longitudinal_utils +{ + +bool isValidTrajectory(const Trajectory & traj) +{ + for (const auto & p : traj.points) { + if ( + !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) || + !isfinite(p.pose.position.z) || !isfinite(p.pose.orientation.w) || + !isfinite(p.pose.orientation.x) || !isfinite(p.pose.orientation.y) || + !isfinite(p.pose.orientation.z) || !isfinite(p.longitudinal_velocity_mps) || + !isfinite(p.lateral_velocity_mps) || !isfinite(p.acceleration_mps2) || + !isfinite(p.heading_rate_rps)) { + return false; + } + } + + // when trajectory is empty + if (traj.points.empty()) { + return false; + } + + return true; +} + +double calcStopDistance( + const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw) +{ + const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); + + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, current_pose, max_dist, max_yaw); + const double signed_length_on_traj = motion_utils::calcSignedArcLength( + traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position, + std::min(end_idx, traj.points.size() - 2)); + + if (std::isnan(signed_length_on_traj)) { + return 0.0; + } + return signed_length_on_traj; +} + +double getPitchByPose(const Quaternion & quaternion_msg) +{ + double roll, pitch, yaw; + tf2::Quaternion quaternion; + tf2::fromMsg(quaternion_msg, quaternion); + tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw); + + return pitch; +} + +double getPitchByTraj( + const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base) +{ + using autoware::common::geometry::distance_2d; + // cannot calculate pitch + if (trajectory.points.size() <= 1) { + return 0.0; + } + + for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { + const double dist = + distance_2d(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + if (dist > wheel_base) { + // calculate pitch from trajectory between rear wheel (nearest) and front center (i) + return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + } + } + + // close to goal + for (size_t i = trajectory.points.size() - 1; i > 0; --i) { + const double dist = distance_2d(trajectory.points.back(), trajectory.points.at(i)); + + if (dist > wheel_base) { + // calculate pitch from trajectory + // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) + return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); + } + } + + // calculate pitch from trajectory between the beginning and end of trajectory + return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); +} + +double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) +{ + const double dx = p_from.pose.position.x - p_to.pose.position.x; + const double dy = p_from.pose.position.y - p_to.pose.position.y; + const double dz = p_from.pose.position.z - p_to.pose.position.z; + + const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); + const double pitch = std::atan2(dz, dxy); + + return pitch; +} + +Pose calcPoseAfterTimeDelay( + const Pose & current_pose, const double delay_time, const double current_vel) +{ + // simple linear prediction + const double yaw = tf2::getYaw(current_pose.orientation); + const double running_distance = delay_time * current_vel; + const double dx = running_distance * std::cos(yaw); + const double dy = running_distance * std::sin(yaw); + + auto pred_pose = current_pose; + pred_pose.position.x += dx; + pred_pose.position.y += dy; + return pred_pose; +} + +double lerp(const double v_from, const double v_to, const double ratio) +{ + return v_from + (v_to - v_from) * ratio; +} + +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} + +double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double max_val, + const double min_val) +{ + const double diff_raw = (input_val - prev_val) / dt; + const double diff = std::min(std::max(diff_raw, min_val), max_val); + const double filtered_val = prev_val + diff * dt; + return filtered_val; +} + +double applyDiffLimitFilter( + const double input_val, const double prev_val, const double dt, const double lim_val) +{ + const double max_val = std::fabs(lim_val); + const double min_val = -max_val; + return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val); +} +} // namespace longitudinal_utils +} // namespace pid_longitudinal_controller +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp similarity index 95% rename from control/trajectory_follower/src/pid.cpp rename to control/pid_longitudinal_controller/src/pid.cpp index a44a3d4694729..b08bb78455a10 100644 --- a/control/trajectory_follower/src/pid.cpp +++ b/control/pid_longitudinal_controller/src/pid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/pid.hpp" +#include "pid_longitudinal_controller/pid.hpp" #include #include @@ -26,7 +26,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { PIDController::PIDController() : m_error_integral(0.0), @@ -108,7 +108,7 @@ void PIDController::reset() m_prev_error = 0.0; m_is_first_time = true; } -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp similarity index 91% rename from control/trajectory_follower/src/pid_longitudinal_controller.cpp rename to control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index bde2139bcaa78..5f8031c425cfd 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower/pid_longitudinal_controller.hpp" +#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "motion_utils/motion_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -30,7 +30,7 @@ namespace motion { namespace control { -namespace trajectory_follower +namespace pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_{&node}, diagnostic_updater_(&node) @@ -103,7 +103,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // set lowpass filter for vel error and pitch const double lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = - std::make_shared(0.0, lpf_vel_error_gain); + std::make_shared(0.0, lpf_vel_error_gain); m_current_vel_threshold_pid_integrate = node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] @@ -171,7 +171,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for slope compensation m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); const double lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; - m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); + m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] @@ -198,44 +198,34 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // diagnostic setupDiagnosticUpdater(); } -void PidLongitudinalController::setInputData(InputData const & input_data) -{ - setTrajectory(input_data.current_trajectory_ptr); - setKinematicState(input_data.current_odometry_ptr); - setCurrentAcceleration(input_data.current_accel_ptr); -} -void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry & msg) { - if (!msg) return; - m_current_kinematic_state_ptr = msg; + m_current_kinematic_state = msg; } void PidLongitudinalController::setCurrentAcceleration( - const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) + const geometry_msgs::msg::AccelWithCovarianceStamped & msg) { - if (!msg) return; - m_current_accel_ptr = msg; + m_current_accel = msg; } void PidLongitudinalController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_auto_planning_msgs::msg::Trajectory & msg) { - if (!msg) return; - - if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) { + if (!pid_longitudinal_controller::longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore."); return; } - if (msg->points.size() < 2) { + if (msg.points.size() < 2) { RCLCPP_WARN_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "Unexpected trajectory size < 2. Ignored."); return; } - m_trajectory_ptr = msg; + m_trajectory = msg; } rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback( @@ -361,15 +351,22 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac return result; } -boost::optional PidLongitudinalController::run() +bool PidLongitudinalController::isReady( + [[maybe_unused]] const trajectory_follower::InputData & input_data) { - // wait for initial pointers - if (!m_current_kinematic_state_ptr || !m_trajectory_ptr || !m_current_accel_ptr) { - return boost::none; - } + return true; +} + +trajectory_follower::LongitudinalOutput PidLongitudinalController::run( + trajectory_follower::InputData const & input_data) +{ + // set input data + setTrajectory(input_data.current_trajectory); + setKinematicState(input_data.current_odometry); + setCurrentAcceleration(input_data.current_accel); // calculate current pose and control data - geometry_msgs::msg::Pose current_pose = m_current_kinematic_state_ptr->pose.pose; + geometry_msgs::msg::Pose current_pose = m_current_kinematic_state.pose.pose; const auto control_data = getControlData(current_pose); @@ -383,7 +380,7 @@ boost::optional PidLongitudinalController::run() const auto cmd_msg = createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command publishDebugData(raw_ctrl_cmd, control_data); // publish debug data - LongitudinalOutput output; + trajectory_follower::LongitudinalOutput output; output.control_cmd = cmd_msg; return output; } @@ -396,7 +393,7 @@ boost::optional PidLongitudinalController::run() // publish control command const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); - LongitudinalOutput output; + trajectory_follower::LongitudinalOutput output; output.control_cmd = cmd_msg; // publish debug data @@ -417,14 +414,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.dt = getDt(); // current velocity and acceleration - control_data.current_motion.vel = m_current_kinematic_state_ptr->twist.twist.linear.x; - control_data.current_motion.acc = m_current_accel_ptr->accel.accel.linear.x; + control_data.current_motion.vel = m_current_kinematic_state.twist.twist.linear.x; + control_data.current_motion.acc = m_current_accel.accel.accel.linear.x; // nearest idx const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_trajectory_ptr->points, current_pose, m_ego_nearest_dist_threshold, - m_ego_nearest_yaw_threshold); - const auto & nearest_point = m_trajectory_ptr->points.at(nearest_idx).pose; + m_trajectory.points, current_pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + const auto & nearest_point = m_trajectory.points.at(nearest_idx).pose; // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = @@ -451,14 +447,14 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData m_prev_shift = control_data.shift; // distance to stopline - control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose, *m_trajectory_ptr, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + control_data.stop_dist = pid_longitudinal_controller::longitudinal_utils::calcStopDistance( + current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch const double raw_pitch = - trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation); - const double traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( - *m_trajectory_ptr, control_data.nearest_idx, m_wheel_base); + pid_longitudinal_controller::longitudinal_utils::getPitchByPose(current_pose.orientation); + const double traj_pitch = pid_longitudinal_controller::longitudinal_utils::getPitchByTraj( + m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); @@ -470,9 +466,9 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm { // These accelerations are without slope compensation const auto & p = m_emergency_state_params; - const double vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + const double vel = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const double acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + const double acc = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); RCLCPP_ERROR_THROTTLE( @@ -512,7 +508,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d static constexpr double vel_epsilon = 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity const double current_vel_cmd = - std::fabs(m_trajectory_ptr->points.at(control_data.nearest_idx).longitudinal_velocity_mps); + std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps); const bool emergency_condition = m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist && current_vel_cmd < vel_epsilon; @@ -626,20 +622,20 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( Motion raw_ctrl_cmd{}; Motion target_motion{}; if (m_control_state == ControlState::DRIVE) { - const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel); - const auto target_interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose); + const auto target_pose = + pid_longitudinal_controller::longitudinal_utils::calcPoseAfterTimeDelay( + current_pose, m_delay_compensation_time, current_vel); + const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, target_interpolated_point.acceleration_mps2}; - target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx); + target_motion = keepBrakeBeforeStop(m_trajectory, target_motion, nearest_idx); const double pred_vel_in_target = predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); m_debug_values.setValues( - trajectory_follower::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); + pid_longitudinal_controller::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); @@ -661,7 +657,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( // This acceleration is without slope compensation const auto & p = m_stopped_state_params; raw_ctrl_cmd.vel = p.vel; - raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( + raw_ctrl_cmd.acc = pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); RCLCPP_DEBUG( @@ -707,7 +703,7 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: void PidLongitudinalController::publishDebugData( const Motion & ctrl_cmd, const ControlData & control_data) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; // set debug values m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); @@ -752,7 +748,7 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift { constexpr double epsilon = 1e-5; - const double target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; + const double target_vel = m_trajectory.points.at(nearest_idx).longitudinal_velocity_mps; if (target_vel > epsilon) { return Shift::Forward; @@ -766,7 +762,7 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift double PidLongitudinalController::calcFilteredAcc( const double raw_acc, const ControlData & control_data) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); @@ -778,8 +774,9 @@ double PidLongitudinalController::calcFilteredAcc( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); // This jerk filter must be applied after slope compensation - const double acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( - acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + const double acc_jerk_filtered = + pid_longitudinal_controller::longitudinal_utils::applyDiffLimitFilter( + acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); return acc_jerk_filtered; @@ -867,7 +864,7 @@ PidLongitudinalController::calcInterpolatedTargetValue( } // apply linear interpolation - return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( + return pid_longitudinal_controller::longitudinal_utils::lerpTrajectoryPoint( traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } @@ -922,7 +919,7 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( double PidLongitudinalController::applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double current_vel_abs = std::fabs(current_vel); const double target_vel_abs = std::fabs(target_motion.vel); const bool enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); @@ -948,7 +945,7 @@ double PidLongitudinalController::applyVelocityFeedback( void PidLongitudinalController::updatePitchDebugValues( const double pitch, const double traj_pitch, const double raw_pitch) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double to_degrees = (180.0 / static_cast(M_PI)); m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); @@ -962,10 +959,10 @@ void PidLongitudinalController::updateDebugVelAcc( const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) { - using trajectory_follower::DebugValues; + using pid_longitudinal_controller::DebugValues; const double current_vel = control_data.current_motion.vel; - const auto interpolated_point = calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose); + const auto interpolated_point = calcInterpolatedTargetValue(m_trajectory, current_pose); m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); @@ -1015,7 +1012,7 @@ void PidLongitudinalController::checkControlState( stat.summary(level, msg); } -} // namespace trajectory_follower +} // namespace pid_longitudinal_controller } // namespace control } // namespace motion } // namespace autoware diff --git a/control/pid_longitudinal_controller/src/smooth_stop.cpp b/control/pid_longitudinal_controller/src/smooth_stop.cpp new file mode 100644 index 0000000000000..db8d81b837012 --- /dev/null +++ b/control/pid_longitudinal_controller/src/smooth_stop.cpp @@ -0,0 +1,170 @@ +// Copyright 2021 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 "pid_longitudinal_controller/smooth_stop.hpp" + +#include // NOLINT + +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace pid_longitudinal_controller +{ +void SmoothStop::init(const double pred_vel_in_target, const double pred_stop_dist) +{ + m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + + // when distance to stopline is near the car + if (pred_stop_dist < std::numeric_limits::epsilon()) { + m_strong_acc = m_params.min_strong_acc; + return; + } + + m_strong_acc = -std::pow(pred_vel_in_target, 2) / (2 * pred_stop_dist); + m_strong_acc = std::max(std::min(m_strong_acc, m_params.max_strong_acc), m_params.min_strong_acc); +} + +void SmoothStop::setParams( + double max_strong_acc, double min_strong_acc, double weak_acc, double weak_stop_acc, + double strong_stop_acc, double min_fast_vel, double min_running_vel, double min_running_acc, + double weak_stop_time, double weak_stop_dist, double strong_stop_dist) +{ + m_params.max_strong_acc = max_strong_acc; + m_params.min_strong_acc = min_strong_acc; + m_params.weak_acc = weak_acc; + m_params.weak_stop_acc = weak_stop_acc; + m_params.strong_stop_acc = strong_stop_acc; + + m_params.min_fast_vel = min_fast_vel; + m_params.min_running_vel = min_running_vel; + m_params.min_running_acc = min_running_acc; + m_params.weak_stop_time = weak_stop_time; + + m_params.weak_stop_dist = weak_stop_dist; + m_params.strong_stop_dist = strong_stop_dist; + + m_is_set_params = true; +} + +std::experimental::optional SmoothStop::calcTimeToStop( + const std::vector> & vel_hist) const +{ + if (!m_is_set_params) { + throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); + } + + // return when vel_hist is empty + const double vel_hist_size = static_cast(vel_hist.size()); + if (vel_hist_size == 0.0) { + return {}; + } + + // calculate some variables for fitting + const rclcpp::Time current_ros_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + double mean_t = 0.0; + double mean_v = 0.0; + double sum_tv = 0.0; + double sum_tt = 0.0; + for (const auto & vel : vel_hist) { + const double t = (vel.first - current_ros_time).seconds(); + const double v = vel.second; + + mean_t += t / vel_hist_size; + mean_v += v / vel_hist_size; + sum_tv += t * v; + sum_tt += t * t; + } + + // return when gradient a (of v = at + b) cannot be calculated. + // See the following calculation of a + if (std::abs(vel_hist_size * mean_t * mean_t - sum_tt) < std::numeric_limits::epsilon()) { + return {}; + } + + // calculate coefficients of linear function (v = at + b) + const double a = + (vel_hist_size * mean_t * mean_v - sum_tv) / (vel_hist_size * mean_t * mean_t - sum_tt); + const double b = mean_v - a * mean_t; + + // return when v is independent of time (v = b) + if (std::abs(a) < std::numeric_limits::epsilon()) { + return {}; + } + + // calculate time to stop by substituting v = 0 for v = at + b + const double time_to_stop = -b / a; + if (time_to_stop > 0) { + return time_to_stop; + } + + return {}; +} + +double SmoothStop::calculate( + const double stop_dist, const double current_vel, const double current_acc, + const std::vector> & vel_hist, const double delay_time) +{ + if (!m_is_set_params) { + throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); + } + + // predict time to stop + const auto time_to_stop = calcTimeToStop(vel_hist); + + // calculate some flags + const bool is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel; + const bool is_running = std::abs(current_vel) > m_params.min_running_vel || + std::abs(current_acc) > m_params.min_running_acc; + + // when exceeding the stopline (stop_dist is negative in these cases.) + if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much + return m_params.strong_stop_acc; + } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit + return m_params.weak_stop_acc; + } + + // when the car is running + if (is_running) { + // when the car will not stop in a certain time + if (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) { + return m_strong_acc; + } else if (!time_to_stop && is_fast_vel) { + return m_strong_acc; + } + + m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + return m_params.weak_acc; + } + + // for 0.5 seconds after the car stopped + if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_weak_acc_time).seconds() < 0.5) { + return m_params.weak_acc; + } + + // when the car is not running + return m_params.strong_stop_acc; +} +} // namespace pid_longitudinal_controller +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp similarity index 99% rename from control/trajectory_follower/test/test_longitudinal_controller_utils.cpp rename to control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 6bd8403b2b514..ef38533376446 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "trajectory_follower/longitudinal_controller_utils.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -30,7 +30,8 @@ #include -namespace longitudinal_utils = ::autoware::motion::control::trajectory_follower::longitudinal_utils; +namespace longitudinal_utils = + ::autoware::motion::control::pid_longitudinal_controller::longitudinal_utils; TEST(TestLongitudinalControllerUtils, isValidTrajectory) { diff --git a/control/trajectory_follower/test/test_pid.cpp b/control/pid_longitudinal_controller/test/test_pid.cpp similarity index 95% rename from control/trajectory_follower/test/test_pid.cpp rename to control/pid_longitudinal_controller/test/test_pid.cpp index 8c7db8f6af255..82d01e0028a9c 100644 --- a/control/trajectory_follower/test/test_pid.cpp +++ b/control/pid_longitudinal_controller/test/test_pid.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include "gtest/gtest.h" -#include "trajectory_follower/pid.hpp" +#include "pid_longitudinal_controller/pid.hpp" #include TEST(TestPID, calculate_pid_output) { - using ::autoware::motion::control::trajectory_follower::PIDController; + using ::autoware::motion::control::pid_longitudinal_controller::PIDController; std::vector contributions; const double dt = 1.0; double target = 10.0; diff --git a/control/trajectory_follower/test/test_smooth_stop.cpp b/control/pid_longitudinal_controller/test/test_smooth_stop.cpp similarity index 96% rename from control/trajectory_follower/test/test_smooth_stop.cpp rename to control/pid_longitudinal_controller/test/test_smooth_stop.cpp index d03ee02c7d109..d658a5271d249 100644 --- a/control/trajectory_follower/test/test_smooth_stop.cpp +++ b/control/pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -13,15 +13,15 @@ // limitations under the License. #include "gtest/gtest.h" +#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/smooth_stop.hpp" #include #include TEST(TestSmoothStop, calculate_stopping_acceleration) { - using ::autoware::motion::control::trajectory_follower::SmoothStop; + using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop; using rclcpp::Duration; using rclcpp::Time; diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index 8150b7b0212dd..e9e57cf08bcc4 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -35,8 +35,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" +#include "trajectory_follower_base/lateral_controller_base.hpp" #include #include @@ -105,12 +104,11 @@ class PurePursuitLateralController : public LateralControllerBase private: rclcpp::Node::SharedPtr node_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - boost::optional> output_tp_array_; + std::vector output_tp_array_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; - nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_; + autoware_auto_planning_msgs::msg::Trajectory trajectory_; + nav_msgs::msg::Odometry current_odometry_; + autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_; boost::optional prev_cmd_; // Debug Publisher @@ -120,8 +118,6 @@ class PurePursuitLateralController : public LateralControllerBase rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; - bool isDataReady(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); @@ -131,22 +127,18 @@ class PurePursuitLateralController : public LateralControllerBase // TF tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::Pose current_pose_; void publishDebugMarker() const; /** * @brief compute control command for path follow with a constant control period */ - boost::optional run() override; + bool isReady([[maybe_unused]] const InputData & input_data) override; + LateralOutput run(const InputData & input_data) override; AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); - /** - * @brief set input data - */ - void setInputData(InputData const & input_data) override; - // Parameter Param param_{}; @@ -166,7 +158,7 @@ class PurePursuitLateralController : public LateralControllerBase boost::optional generatePredictedTrajectory(); - boost::optional generateOutputControlCmd(); + AckermannLateralCommand generateOutputControlCmd(); bool calcIsSteerConverged(const AckermannLateralCommand & cmd); diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 222d75aa833bf..d2b3ed292c7c6 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -31,7 +31,7 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - trajectory_follower + trajectory_follower_base vehicle_info_util visualization_msgs diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index 7067e89f7e748..5cf4ee07990b2 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -57,7 +57,7 @@ enum TYPE { namespace pure_pursuit { PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) -: node_{&node}, self_pose_listener_(&node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) +: node_{&node}, tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) { pure_pursuit_ = std::make_unique(); @@ -101,35 +101,6 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); } -bool PurePursuitLateralController::isDataReady() -{ - if (!current_odometry_) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_odometry..."); - return false; - } - - if (!trajectory_) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "waiting for trajectory..."); - return false; - } - - if (!current_pose_) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_pose..."); - return false; - } - - if (!current_steering_) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_steering..."); - return false; - } - - return true; -} - double PurePursuitLateralController::calcLookaheadDistance( const double lateral_error, const double curvature, const double velocity, const double min_ld, const bool is_control_cmd) @@ -168,13 +139,6 @@ double PurePursuitLateralController::calcLookaheadDistance( return total_ld; } -void PurePursuitLateralController::setInputData(InputData const & input_data) -{ - trajectory_ = input_data.current_trajectory_ptr; - current_odometry_ = input_data.current_odometry_ptr; - current_steering_ = input_data.current_steering_ptr; -} - TrajectoryPoint PurePursuitLateralController::calcNextPose( const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const { @@ -196,7 +160,7 @@ void PurePursuitLateralController::setResampledTrajectory() { // Interpolate with constant interval distance. std::vector out_arclength; - const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(*trajectory_); + const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_); const auto traj_length = motion_utils::calcArcLength(input_tp_array); for (double s = 0; s < traj_length; s += param_.resampling_ds) { out_arclength.push_back(s); @@ -204,10 +168,9 @@ void PurePursuitLateralController::setResampledTrajectory() trajectory_resampled_ = std::make_shared(motion_utils::resampleTrajectory( motion_utils::convertToTrajectory(input_tp_array), out_arclength)); - trajectory_resampled_->points.back() = trajectory_->points.back(); - trajectory_resampled_->header = trajectory_->header; - output_tp_array_ = boost::optional>( - motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_)); + trajectory_resampled_->points.back() = trajectory_.points.back(); + trajectory_resampled_->header = trajectory_.header; + output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); } double PurePursuitLateralController::calcCurvature(const size_t closest_idx) @@ -298,7 +261,7 @@ void PurePursuitLateralController::averageFilterTrajectory( boost::optional PurePursuitLateralController::generatePredictedTrajectory() { const auto closest_idx_result = - motion_utils::findNearestIndex(*output_tp_array_, current_pose_->pose, 3.0, M_PI_4); + motion_utils::findNearestIndex(output_tp_array_, current_pose_, 3.0, M_PI_4); if (!closest_idx_result) { return boost::none; @@ -319,8 +282,8 @@ boost::optional PurePursuitLateralController::generatePredictedTraje // For first point, use the odometry for velocity, and use the current_pose for prediction. TrajectoryPoint p; - p.pose = current_pose_->pose; - p.longitudinal_velocity_mps = current_odometry_->twist.twist.linear.x; + p.pose = current_pose_; + p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x; predicted_trajectory.points.push_back(p); const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); @@ -365,27 +328,27 @@ boost::optional PurePursuitLateralController::generatePredictedTraje return predicted_trajectory; } -boost::optional PurePursuitLateralController::run() +bool PurePursuitLateralController::isReady([[maybe_unused]] const InputData & input_data) { - current_pose_ = self_pose_listener_.getCurrentPose(); - if (!isDataReady()) { - return boost::none; - } + return true; +} + +LateralOutput PurePursuitLateralController::run(const InputData & input_data) +{ + current_pose_ = input_data.current_odometry.pose.pose; + trajectory_ = input_data.current_trajectory; + current_odometry_ = input_data.current_odometry; + current_steering_ = input_data.current_steering; + setResampledTrajectory(); - if (!output_tp_array_ || !trajectory_resampled_) { - return boost::none; - } if (param_.enable_path_smoothing) { averageFilterTrajectory(*trajectory_resampled_); } const auto cmd_msg = generateOutputControlCmd(); - if (!cmd_msg) { - RCLCPP_ERROR(node_->get_logger(), "Failed to generate control command."); - return boost::none; - } + LateralOutput output; - output.control_cmd = *cmd_msg; - output.sync_data.is_steer_converged = calcIsSteerConverged(*cmd_msg); + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg); // calculate predicted trajectory with iterative calculation const auto predicted_trajectory = generatePredictedTrajectory(); @@ -400,14 +363,14 @@ boost::optional PurePursuitLateralController::run() bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) { - return std::abs(cmd.steering_tire_angle - current_steering_->steering_tire_angle) < + return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < static_cast(param_.converged_steer_rad_); } -boost::optional PurePursuitLateralController::generateOutputControlCmd() +AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() { // Generate the control command - const auto pp_output = calcTargetCurvature(true, current_pose_->pose); + const auto pp_output = calcTargetCurvature(true, current_pose_); AckermannLateralCommand output_cmd; if (pp_output) { @@ -447,7 +410,7 @@ void PurePursuitLateralController::publishDebugMarker() const marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); marker_array.markers.push_back( - createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose)); + createTrajectoryCircleMarker(debug_data_.next_target, current_pose_)); pub_debug_marker_->publish(marker_array); } @@ -465,7 +428,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( // Calculate target point for velocity/acceleration const auto closest_idx_result = - motion_utils::findNearestIndex(*output_tp_array_, pose, 3.0, M_PI_4); + motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); if (!closest_idx_result) { RCLCPP_ERROR(node_->get_logger(), "cannot find closest waypoint"); return {}; @@ -491,7 +454,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( double lookahead_distance = min_lookahead_distance; if (is_control_output) { lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, current_odometry_->twist.twist.linear.x, + lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, min_lookahead_distance, is_control_output); } else { lookahead_distance = calcLookaheadDistance( @@ -518,7 +481,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( PpOutput output{}; output.curvature = kappa; if (!is_control_output) { - output.velocity = current_odometry_->twist.twist.linear.x; + output.velocity = current_odometry_.twist.twist.linear.x; } else { output.velocity = target_vel; } diff --git a/control/trajectory_follower/CMakeLists.txt b/control/trajectory_follower/CMakeLists.txt deleted file mode 100644 index 52e46781f0ab7..0000000000000 --- a/control/trajectory_follower/CMakeLists.txt +++ /dev/null @@ -1,98 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trajectory_follower) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# lateral_controller -set(LATERAL_CONTROLLER_LIB lateral_controller_lib) -set(LATERAL_CONTROLLER_LIB_SRC - src/mpc_lateral_controller.cpp - src/interpolate.cpp - src/lowpass_filter.cpp - src/mpc.cpp - src/mpc_trajectory.cpp - src/mpc_utils.cpp - src/qp_solver/qp_solver_osqp.cpp - src/qp_solver/qp_solver_unconstr_fast.cpp - src/vehicle_model/vehicle_model_bicycle_dynamics.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/vehicle_model/vehicle_model_interface.cpp -) - -set(LATERAL_CONTROLLER_LIB_HEADERS - include/trajectory_follower/lateral_controller_base.hpp - include/trajectory_follower/mpc_lateral_controller.hpp - include/trajectory_follower/sync_data.hpp - include/trajectory_follower/input_data.hpp - include/trajectory_follower/visibility_control.hpp - include/trajectory_follower/interpolate.hpp - include/trajectory_follower/lowpass_filter.hpp - include/trajectory_follower/mpc.hpp - include/trajectory_follower/mpc_trajectory.hpp - include/trajectory_follower/mpc_utils.hpp - include/trajectory_follower/qp_solver/qp_solver_interface.hpp - include/trajectory_follower/qp_solver/qp_solver_osqp.hpp - include/trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp - include/trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp - include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp - include/trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp - include/trajectory_follower/vehicle_model/vehicle_model_interface.hpp -) - -ament_auto_add_library(${LATERAL_CONTROLLER_LIB} SHARED - ${LATERAL_CONTROLLER_LIB_SRC} - ${LATERAL_CONTROLLER_LIB_HEADERS} -) -target_compile_options(${LATERAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast) - -# longitudinal_controller -set(LONGITUDINAL_CONTROLLER_LIB longitudinal_controller_lib) -set(LONGITUDINAL_CONTROLLER_LIB_SRC - src/pid_longitudinal_controller.cpp - src/pid.cpp - src/smooth_stop.cpp - src/longitudinal_controller_utils.cpp -) - -set(LONGITUDINAL_CONTROLLER_LIB_HEADERS - include/trajectory_follower/longitudinal_controller_base.hpp - include/trajectory_follower/pid_longitudinal_controller.hpp - include/trajectory_follower/sync_data.hpp - include/trajectory_follower/input_data.hpp - include/trajectory_follower/debug_values.hpp - include/trajectory_follower/pid.hpp - include/trajectory_follower/smooth_stop.hpp - include/trajectory_follower/longitudinal_controller_utils.hpp -) - -ament_auto_add_library(${LONGITUDINAL_CONTROLLER_LIB} SHARED - ${LONGITUDINAL_CONTROLLER_LIB_SRC} - ${LONGITUDINAL_CONTROLLER_LIB_HEADERS} -) -target_compile_options(${LONGITUDINAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-cast) - -if(BUILD_TESTING) - set(TEST_LAT_SOURCES - test/test_mpc.cpp - test/test_mpc_utils.cpp - test/test_interpolate.cpp - test/test_lowpass_filter.cpp - ) - set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) - ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) - target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${LATERAL_CONTROLLER_LIB}) - - set(TEST_LON_SOURCES - test/test_pid.cpp - test/test_smooth_stop.cpp - test/test_longitudinal_controller_utils.cpp - ) - set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) - ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) - target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${LONGITUDINAL_CONTROLLER_LIB}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/control/trajectory_follower_base/CMakeLists.txt b/control/trajectory_follower_base/CMakeLists.txt new file mode 100644 index 0000000000000..98f86e468e819 --- /dev/null +++ b/control/trajectory_follower_base/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(trajectory_follower_base) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/longitudinal_controller_base.cpp + src/lateral_controller_base.cpp +) + +ament_auto_package() diff --git a/control/trajectory_follower/design/trajectory_follower-design.md b/control/trajectory_follower_base/README.md similarity index 69% rename from control/trajectory_follower/design/trajectory_follower-design.md rename to control/trajectory_follower_base/README.md index 46358abe91090..6bd3d74e75271 100644 --- a/control/trajectory_follower/design/trajectory_follower-design.md +++ b/control/trajectory_follower_base/README.md @@ -8,29 +8,22 @@ This is the design document for the `trajectory_follower` package. -This package provides the library code used by the nodes of the `trajectory_follower_nodes` package. -Mainly, it implements two algorithms: - -- Model-Predictive Control (MPC) for the computation of lateral steering commands. - - [trajectory_follower-mpc-design](mpc_lateral_controller-design.md) -- PID control for the computation of velocity and acceleration commands. - - [trajectory_follower-pid-design](pid_longitudinal_controller-design.md) +This package provides the interface of longitudinal and lateral controllers used by the node of the `trajectory_follower_node` package. +We can implement a detailed controller by deriving the longitudinal and lateral base interfaces. ## Design -![Controller](../../trajectory_follower_nodes/design/media/Controller.drawio.svg) - There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. The interface class has the following base functions. -- `setInputData()`: Input the data subscribed in [Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented with the inherited algorithm and the used data must be selected. -- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented by inherited algorithms. -- `syncData()`: Input the result of running the other controller. +- `isReady()`: Check if the control is ready to compute. +- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_node/README.md). This must be implemented by inherited algorithms. +- `sync()`: Input the result of running the other controller. - steer angle convergence - allow keeping stopped until steer is converged. - velocity convergence(currently not used) -See [the Design of Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md#Design) for how these functions work in the node. +See [the Design of Trajectory Follower Nodes](../trajectory_follower_node/README.md#Design) for how these functions work in the node. ## Separated lateral (steering) and longitudinal (velocity) controls diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp similarity index 70% rename from control/trajectory_follower/include/trajectory_follower/input_data.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index b356095e2f78e..b135f5034d103 100644 --- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ -#define TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" @@ -30,14 +30,14 @@ namespace trajectory_follower { struct InputData { - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr; - nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr; - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr; - geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr; + autoware_auto_planning_msgs::msg::Trajectory current_trajectory; + nav_msgs::msg::Odometry current_odometry; + autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; + geometry_msgs::msg::AccelWithCovarianceStamped current_accel; }; } // namespace trajectory_follower } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp similarity index 73% rename from control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp index 0a234fa0b42b1..28eddd175e38e 100644 --- a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/input_data.hpp" -#include "trajectory_follower/sync_data.hpp" +#include "trajectory_follower_base/input_data.hpp" +#include "trajectory_follower_base/sync_data.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -40,12 +40,9 @@ struct LateralOutput class LateralControllerBase { public: - virtual boost::optional run() = 0; - virtual void setInputData(InputData const & input_data) = 0; - void sync(LongitudinalSyncData const & longitudinal_sync_data) - { - longitudinal_sync_data_ = longitudinal_sync_data; - }; + virtual bool isReady(const InputData & input_data) = 0; + virtual LateralOutput run(InputData const & input_data) = 0; + void sync(LongitudinalSyncData const & longitudinal_sync_data); protected: LongitudinalSyncData longitudinal_sync_data_; @@ -56,4 +53,4 @@ class LateralControllerBase } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp similarity index 64% rename from control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index 6eb4d13119275..5da298fe32b12 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower/input_data.hpp" -#include "trajectory_follower/sync_data.hpp" +#include "trajectory_follower_base/input_data.hpp" +#include "trajectory_follower_base/sync_data.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" @@ -39,9 +39,12 @@ struct LongitudinalOutput class LongitudinalControllerBase { public: - virtual boost::optional run() = 0; - virtual void setInputData(InputData const & input_data) = 0; - void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; } + virtual bool isReady(const InputData & input_data) = 0; + virtual LongitudinalOutput run(InputData const & input_data) = 0; + void sync(LateralSyncData const & lateral_sync_data); + // NOTE: This reset function should be called when the trajectory is replaned by changing ego pose + // or goal pose. + void reset(); protected: LateralSyncData lateral_sync_data_; @@ -52,4 +55,4 @@ class LongitudinalControllerBase } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp similarity index 79% rename from control/trajectory_follower/include/trajectory_follower/sync_data.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp index 48224a5498a9c..4735fc4a3e842 100644 --- a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ -#define TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ namespace autoware { @@ -30,7 +30,8 @@ struct LateralSyncData struct LongitudinalSyncData { - bool is_velocity_converged{false}; + // NOTE: This variable is not used for now + // bool is_velocity_converged{false}; }; } // namespace trajectory_follower @@ -38,4 +39,4 @@ struct LongitudinalSyncData } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp similarity index 89% rename from control/trajectory_follower/include/trajectory_follower/visibility_control.hpp rename to control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp index b501ec9a55264..c2419427ac961 100644 --- a/control/trajectory_follower/include/trajectory_follower/visibility_control.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER__VISIBILITY_CONTROL_HPP_ +#ifndef TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#define TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // TRAJECTORY_FOLLOWER__VISIBILITY_CONTROL_HPP_ +#endif // TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower_base/package.xml similarity index 97% rename from control/trajectory_follower/package.xml rename to control/trajectory_follower_base/package.xml index b6de5384180ec..7482cbf190be3 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -1,7 +1,7 @@ - trajectory_follower + trajectory_follower_base 1.0.0 Library for generating lateral and longitudinal controls following a trajectory diff --git a/control/trajectory_follower_base/src/lateral_controller_base.cpp b/control/trajectory_follower_base/src/lateral_controller_base.cpp new file mode 100644 index 0000000000000..1fe2f77d8b074 --- /dev/null +++ b/control/trajectory_follower_base/src/lateral_controller_base.cpp @@ -0,0 +1,33 @@ +// Copyright 2022 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "trajectory_follower_base/lateral_controller_base.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +void LateralControllerBase::sync(LongitudinalSyncData const & longitudinal_sync_data) +{ + longitudinal_sync_data_ = longitudinal_sync_data; +} + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp new file mode 100644 index 0000000000000..70fe47ee9d54a --- /dev/null +++ b/control/trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -0,0 +1,33 @@ +// Copyright 2022 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "trajectory_follower_base/longitudinal_controller_base.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +void LongitudinalControllerBase::sync(LateralSyncData const & lateral_sync_data) +{ + lateral_sync_data_ = lateral_sync_data; +} +void LongitudinalControllerBase::reset() { lateral_sync_data_.is_steer_converged = false; } +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt similarity index 83% rename from control/trajectory_follower_nodes/CMakeLists.txt rename to control/trajectory_follower_node/CMakeLists.txt index b40bd8f16a626..a2316a6db30d2 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_node/CMakeLists.txt @@ -1,25 +1,25 @@ cmake_minimum_required(VERSION 3.14) -project(trajectory_follower_nodes) +project(trajectory_follower_node) find_package(autoware_cmake REQUIRED) autoware_package() set(CONTROLLER_NODE controller_node) ament_auto_add_library(${CONTROLLER_NODE} SHARED - include/trajectory_follower_nodes/controller_node.hpp + include/trajectory_follower_node/controller_node.hpp src/controller_node.cpp ) target_compile_options(${CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${CONTROLLER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_nodes::Controller" + PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" EXECUTABLE ${CONTROLLER_NODE}_exe ) # simple trajectory follower set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED - include/trajectory_follower_nodes/simple_trajectory_follower.hpp + include/trajectory_follower_node/simple_trajectory_follower.hpp src/simple_trajectory_follower.cpp ) @@ -32,7 +32,7 @@ rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} ) if(BUILD_TESTING) - set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) + set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_node) ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} test/trajectory_follower_test_utils.hpp test/test_controller_node.cpp @@ -44,7 +44,7 @@ if(BUILD_TESTING) find_package(autoware_testing REQUIRED) add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe PARAM_FILENAMES "lateral_controller_defaults.param.yaml longitudinal_controller_defaults.param.yaml -test_vehicle_info.param.yaml test_nearest_search.param.yaml" +test_vehicle_info.param.yaml test_nearest_search.param.yaml trajectory_follower_node.param.yaml" ) endif() diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md new file mode 100644 index 0000000000000..a98804f12d796 --- /dev/null +++ b/control/trajectory_follower_node/README.md @@ -0,0 +1,156 @@ +# Trajectory Follower Nodes + +## Purpose + +Generate control commands to follow a given Trajectory. + +## Design + +This is a node of the functionalities implemented in the controller class derived from [trajectory_follower_base](../trajectory_follower_base/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. + +By default, the controller instance with the `Controller` class as follows is used. + +```plantuml +@startuml +package trajectory_follower_base { +abstract class LateralControllerBase { +longitudinal_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LongitudinalSyncData) + reset() + +} +abstract class LongitudinalControllerBase { +lateral_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LateralSyncData) + reset() + +} + +struct InputData { +trajectory +odometry +steering +accel +} +struct LongitudinalSyncData { +is_steer_converged +} +struct LateralSyncData { +} +} + +package mpc_lateral_controller { +class MPCLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pure_pursuit { +class PurePursuitLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pid_longitudinal_controller { +class PIDLongitudinalController { +isReady(InputData) override +run(InputData) override +} +} + +package trajectory_follower_node { +class Controller { +longitudinal_controller_ +lateral_controller_ +onTimer() +createInputData(): InputData +} +} + +MPCLateralController --|> LateralControllerBase +PurePursuitLateralController --|> LateralControllerBase +PIDLongitudinalController --|> LongitudinalControllerBase + +LateralSyncData --> LongitudinalControllerBase +LateralSyncData --> LateralControllerBase +LongitudinalSyncData --> LongitudinalControllerBase +LongitudinalSyncData --> LateralControllerBase +InputData ..> LateralControllerBase +InputData ..> LongitudinalControllerBase + +LateralControllerBase --o Controller +LongitudinalControllerBase --o Controller +InputData ..> Controller +@enduml +``` + +The process flow of `Controller` class is as follows. + +```cpp +// 1. create input data +const auto input_data = createInputData(*get_clock()); +if (!input_data) { + return; +} + +// 2. check if controllers are ready +const bool is_lat_ready = lateral_controller_->isReady(*input_data); +const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); +if (!is_lat_ready || !is_lon_ready) { + return; +} + +// 3. run controllers +const auto lat_out = lateral_controller_->run(*input_data); +const auto lon_out = longitudinal_controller_->run(*input_data); + +// 4. sync with each other controllers +longitudinal_controller_->sync(lat_out.sync_data); +lateral_controller_->sync(lon_out.sync_data); + +// 5. publish control command +control_cmd_pub_->publish(out); +``` + +Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` + +- lateral controller + - `keep_steer_control_until_converged` +- longitudinal controller + - `enable_keep_stopped_until_steer_convergence` + +### Inputs / Outputs / API + +#### Inputs + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_auto_vehicle_msgs/SteeringReport` current steering + +#### Outputs + +- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. + +#### Parameter + +- `ctrl_period`: control commands publishing period +- `timeout_thr_sec`: duration in second after which input messages are discarded. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + 1. Both commands have been received. + 2. The last received commands are not older than defined by `timeout_thr_sec`. +- `lateral_controller_mode`: `mpc_follower` or `pure_pursuit` + - (currently there is only `PID` for longitudinal controller) + +## Debugging + +Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. + +A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. + +In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml b/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml similarity index 100% rename from control/trajectory_follower_nodes/config/plot_juggler_trajectory_follower.xml rename to control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml diff --git a/control/trajectory_follower_nodes/design/media/Controller.drawio.svg b/control/trajectory_follower_node/design/media/Controller.drawio.svg similarity index 99% rename from control/trajectory_follower_nodes/design/media/Controller.drawio.svg rename to control/trajectory_follower_node/design/media/Controller.drawio.svg index 7da747243d061..1152ef2b17f59 100644 --- a/control/trajectory_follower_nodes/design/media/Controller.drawio.svg +++ b/control/trajectory_follower_node/design/media/Controller.drawio.svg @@ -83,12 +83,12 @@
- trajectory_follower_nodes/controller_node + trajectory_follower_node/controller_node
- trajectory_follower_nodes/controller_node + trajectory_follower_node/controller_node diff --git a/control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md similarity index 100% rename from control/trajectory_follower_nodes/design/simple_trajectory_follower-design.md rename to control/trajectory_follower_node/design/simple_trajectory_follower-design.md diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp similarity index 77% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp rename to control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index f8493c7a1cb28..8d46732679af8 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ +#ifndef TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -21,19 +21,14 @@ #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "trajectory_follower/debug_values.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" -#include "trajectory_follower/longitudinal_controller_base.hpp" -#include "trajectory_follower/longitudinal_controller_utils.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/pid.hpp" -#include "trajectory_follower/smooth_stop.hpp" +#include "trajectory_follower_base/lateral_controller_base.hpp" +#include "trajectory_follower_base/longitudinal_controller_base.hpp" +#include "trajectory_follower_node/visibility_control.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" @@ -54,7 +49,7 @@ namespace control { using trajectory_follower::LateralOutput; using trajectory_follower::LongitudinalOutput; -namespace trajectory_follower_nodes +namespace trajectory_follower_node { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -69,10 +64,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node private: rclcpp::TimerBase::SharedPtr timer_control_; - trajectory_follower::InputData input_data_; double timeout_thr_sec_; boost::optional longitudinal_output_{boost::none}; - boost::optional lateral_output_{boost::none}; std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; @@ -85,6 +78,11 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node control_cmd_pub_; rclcpp::Publisher::SharedPtr debug_marker_pub_; + autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; + nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; + autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; + enum class LateralControllerMode { INVALID = 0, MPC = 1, @@ -98,20 +96,23 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node /** * @brief compute control command, and publish periodically */ + boost::optional createInputData(rclcpp::Clock & clock) const; void callbackTimerControl(); void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - bool isTimeOut(); + bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( const std::string & algorithm_name) const; - void publishDebugMarker() const; + void publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const; }; -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower_node } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER_NODES__CONTROLLER_NODE_HPP_ +#endif // TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp similarity index 91% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp rename to control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp index a862477f309ab..e748bdf25d419 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#ifndef TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ #include @@ -63,4 +63,4 @@ class SimpleTrajectoryFollower : public rclcpp::Node } // namespace simple_trajectory_follower -#endif // TRAJECTORY_FOLLOWER_NODES__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#endif // TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp similarity index 89% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp rename to control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp index e4f75f3e456b6..36446c4144a0a 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/visibility_control.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ +#ifndef TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#define TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // TRAJECTORY_FOLLOWER_NODES__VISIBILITY_CONTROL_HPP_ +#endif // TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml similarity index 84% rename from control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml rename to control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml index f74fa994086b2..8ce799e17e632 100644 --- a/control/trajectory_follower_nodes/launch/simple_trajectory_follower.launch.xml +++ b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_node/package.xml similarity index 90% rename from control/trajectory_follower_nodes/package.xml rename to control/trajectory_follower_node/package.xml index de184228e5f87..7a32ac96d1555 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -1,7 +1,7 @@ - trajectory_follower_nodes + trajectory_follower_node 1.0.0 Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands @@ -25,10 +25,12 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs motion_utils + mpc_lateral_controller + pid_longitudinal_controller pure_pursuit rclcpp rclcpp_components - trajectory_follower + trajectory_follower_base vehicle_info_util ros2launch diff --git a/control/trajectory_follower_node/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_node/param/lateral_controller_defaults.param.yaml new file mode 100644 index 0000000000000..92d15da9e49e6 --- /dev/null +++ b/control/trajectory_follower_node/param/lateral_controller_defaults.param.yaml @@ -0,0 +1,73 @@ +/**: + ros__parameters: + # -- system -- + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + + # -- path smoothing -- + enable_path_smoothing: false # flag for path smoothing + path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing + curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + + # -- trajectory extending -- + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + + # -- mpc optimization -- + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length + + # -- vehicle model -- + vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + + # -- lowpass filter for noise reduction -- + steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + error_deriv_lpf_cutoff_hz: 5.0 + + # stop state: steering command is kept in the previous value in the stop state. + stop_state_entry_ego_speed: 0.001 + stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: true + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 + + # vehicle parameters + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/control/trajectory_follower_node/param/longitudinal_controller_defaults.param.yaml similarity index 88% rename from launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml rename to control/trajectory_follower_node/param/longitudinal_controller_defaults.param.yaml index b6e1c3a38c799..eb2ef443c4576 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/control/trajectory_follower_node/param/longitudinal_controller_defaults.param.yaml @@ -72,15 +72,3 @@ lpf_pitch_gain: 0.95 max_pitch_rad: 0.1 min_pitch_rad: -0.1 - - # vehicle parameters - vehicle: - cg_to_front_m: 1.228 - cg_to_rear_m: 1.5618 - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 diff --git a/control/trajectory_follower_nodes/param/test_nearest_search.param.yaml b/control/trajectory_follower_node/param/test_nearest_search.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/test_nearest_search.param.yaml rename to control/trajectory_follower_node/param/test_nearest_search.param.yaml diff --git a/control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml b/control/trajectory_follower_node/param/test_vehicle_info.param.yaml similarity index 100% rename from control/trajectory_follower_nodes/param/test_vehicle_info.param.yaml rename to control/trajectory_follower_node/param/test_vehicle_info.param.yaml diff --git a/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml b/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml new file mode 100644 index 0000000000000..dcd985bb4a6b1 --- /dev/null +++ b/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + ctrl_period: 0.03 + timeout_thr_sec: 0.5 diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp similarity index 58% rename from control/trajectory_follower_nodes/src/controller_node.cpp rename to control/trajectory_follower_node/src/controller_node.cpp index 8b168afc08416..7b98a69a7c3a2 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_nodes/controller_node.hpp" +#include "trajectory_follower_node/controller_node.hpp" +#include "mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "pure_pursuit/pure_pursuit_lateral_controller.hpp" -#include "trajectory_follower/mpc_lateral_controller.hpp" -#include "trajectory_follower/pid_longitudinal_controller.hpp" #include #include @@ -31,20 +31,20 @@ namespace motion { namespace control { -namespace trajectory_follower_nodes +namespace trajectory_follower_node { Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) { using std::placeholders::_1; - const double ctrl_period = declare_parameter("ctrl_period", 0.03); - timeout_thr_sec_ = declare_parameter("timeout_thr_sec", 0.5); + const double ctrl_period = declare_parameter("ctrl_period"); + timeout_thr_sec_ = declare_parameter("timeout_thr_sec"); const auto lateral_controller_mode = - getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc_follower")); + getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc")); switch (lateral_controller_mode) { case LateralControllerMode::MPC: { - lateral_controller_ = std::make_shared(*this); + lateral_controller_ = std::make_shared(*this); break; } case LateralControllerMode::PURE_PURSUIT: { @@ -55,12 +55,12 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LateralController] invalid algorithm"); } - const auto longitudinal_controller_mode = - getLongitudinalControllerMode(declare_parameter("longitudinal_controller_mode", "pid")); + const auto longitudinal_controller_mode = getLongitudinalControllerMode( + declare_parameter("longitudinal_controller_mode", "pid")); switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = - std::make_shared(*this); + std::make_shared(*this); break; } default: @@ -92,7 +92,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control Controller::LateralControllerMode Controller::getLateralControllerMode( const std::string & controller_mode) const { - if (controller_mode == "mpc_follower") return LateralControllerMode::MPC; + if (controller_mode == "mpc") return LateralControllerMode::MPC; if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT; return LateralControllerMode::INVALID; @@ -108,34 +108,36 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) { - input_data_.current_trajectory_ptr = msg; + current_trajectory_ptr_ = msg; } void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) { - input_data_.current_odometry_ptr = msg; + current_odometry_ptr_ = msg; } void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { - input_data_.current_steering_ptr = msg; + current_steering_ptr_ = msg; } void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) { - input_data_.current_accel_ptr = msg; + current_accel_ptr_ = msg; } -bool Controller::isTimeOut() +bool Controller::isTimeOut( + const trajectory_follower::LongitudinalOutput & lon_out, + const trajectory_follower::LateralOutput & lat_out) { const auto now = this->now(); - if ((now - lateral_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { + if ((now - lat_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000 /*ms*/, "Lateral control command too old, control_cmd will not be published."); return true; } - if ((now - longitudinal_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { + if ((now - lon_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000 /*ms*/, "Longitudinal control command too old, control_cmd will not be published."); @@ -144,37 +146,83 @@ bool Controller::isTimeOut() return false; } +boost::optional Controller::createInputData( + rclcpp::Clock & clock) const +{ + if (!current_trajectory_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory."); + return {}; + } + + if (!current_odometry_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry."); + return {}; + } + + if (!current_steering_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering."); + return {}; + } + + if (!current_accel_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel."); + return {}; + } + + trajectory_follower::InputData input_data; + input_data.current_trajectory = *current_trajectory_ptr_; + input_data.current_odometry = *current_odometry_ptr_; + input_data.current_steering = *current_steering_ptr_; + input_data.current_accel = *current_accel_ptr_; + + return input_data; +} + void Controller::callbackTimerControl() { - // Since the longitudinal uses the convergence information of the steer - // with the current trajectory, it is necessary to run the lateral first. - // TODO(kosuke55): Do not depend on the order of execution. - lateral_controller_->setInputData(input_data_); // trajectory, odometry, steering - const auto lat_out = lateral_controller_->run(); - lateral_output_ = lat_out ? lat_out : lateral_output_; // use previous value if none. - if (!lateral_output_) return; - - longitudinal_controller_->sync(lateral_output_->sync_data); - longitudinal_controller_->setInputData(input_data_); // trajectory, odometry - const auto lon_out = longitudinal_controller_->run(); - longitudinal_output_ = lon_out ? lon_out : longitudinal_output_; // use previous value if none. - if (!longitudinal_output_) return; - - lateral_controller_->sync(longitudinal_output_->sync_data); + // 1. create input data + const auto input_data = createInputData(*get_clock()); + if (!input_data) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "Control is skipped since input data is not ready."); + return; + } + + // 2. check if controllers are ready + const bool is_lat_ready = lateral_controller_->isReady(*input_data); + const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); + if (!is_lat_ready || !is_lon_ready) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Control is skipped since lateral and/or longitudinal controllers are not ready to run."); + return; + } + + // 3. run controllers + const auto lat_out = lateral_controller_->run(*input_data); + const auto lon_out = longitudinal_controller_->run(*input_data); + + // 4. sync with each other controllers + longitudinal_controller_->sync(lat_out.sync_data); + lateral_controller_->sync(lon_out.sync_data); // TODO(Horibe): Think specification. This comes from the old implementation. - if (isTimeOut()) return; + if (isTimeOut(lon_out, lat_out)) return; + // 5. publish control command autoware_auto_control_msgs::msg::AckermannControlCommand out; out.stamp = this->now(); - out.lateral = lateral_output_->control_cmd; - out.longitudinal = longitudinal_output_->control_cmd; + out.lateral = lat_out.control_cmd; + out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - publishDebugMarker(); + // 6. publish debug marker + publishDebugMarker(*input_data, lat_out); } -void Controller::publishDebugMarker() const +void Controller::publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const { visualization_msgs::msg::MarkerArray debug_marker_array{}; @@ -184,14 +232,14 @@ void Controller::publishDebugMarker() const "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); - marker.pose = input_data_.current_odometry_ptr->pose.pose; + marker.pose = input_data.current_odometry.pose.pose; std::stringstream ss; - const double current = input_data_.current_steering_ptr->steering_tire_angle; - const double cmd = lateral_output_->control_cmd.steering_tire_angle; + const double current = input_data.current_steering.steering_tire_angle; + const double cmd = lat_out.control_cmd.steering_tire_angle; const double diff = current - cmd; ss << "current:" << current << " cmd:" << cmd << " diff:" << diff - << (lateral_output_->sync_data.is_steer_converged ? " (converged)" : " (not converged)"); + << (lat_out.sync_data.is_steer_converged ? " (converged)" : " (not converged)"); marker.text = ss.str(); debug_marker_array.markers.push_back(marker); @@ -200,10 +248,10 @@ void Controller::publishDebugMarker() const debug_marker_pub_->publish(debug_marker_array); } -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower_node } // namespace control } // namespace motion } // namespace autoware #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_nodes::Controller) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_node::Controller) diff --git a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp similarity index 98% rename from control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp rename to control/trajectory_follower_node/src/simple_trajectory_follower.cpp index ecaa12398a68e..0af901be16125 100644 --- a/control/trajectory_follower_nodes/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_nodes/simple_trajectory_follower.hpp" +#include "trajectory_follower_node/simple_trajectory_follower.hpp" #include #include diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp similarity index 77% rename from control/trajectory_follower_nodes/test/test_controller_node.cpp rename to control/trajectory_follower_node/test/test_controller_node.cpp index 8ea882d64aad1..d3b39a018652c 100644 --- a/control/trajectory_follower_nodes/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -17,7 +17,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" -#include "trajectory_follower_nodes/controller_node.hpp" +#include "trajectory_follower_node/controller_node.hpp" #include "trajectory_follower_test_utils.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -33,7 +33,7 @@ #include #include -using Controller = autoware::motion::control::trajectory_follower_nodes::Controller; +using Controller = autoware::motion::control::trajectory_follower_node::Controller; using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -45,20 +45,35 @@ using FakeNodeFixture = autoware::tools::testing::FakeTestNode; const rclcpp::Duration one_second(1, 0); -std::shared_ptr makeNode() +rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_convergence = false) { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); + const auto longitudinal_share_dir = + ament_index_cpp::get_package_share_directory("pid_longitudinal_controller"); + const auto lateral_share_dir = + ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); rclcpp::NodeOptions node_options; node_options.append_parameter_override("ctrl_period", 0.03); node_options.append_parameter_override("timeout_thr_sec", 0.5); + node_options.append_parameter_override("lateral_controller_mode", "mpc"); + node_options.append_parameter_override("longitudinal_controller_mode", "pid"); node_options.append_parameter_override( - "enable_keep_stopped_until_steer_convergence", false); // longitudinal + "enable_keep_stopped_until_steer_convergence", + enable_keep_stopped_until_steer_convergence); // longitudinal node_options.arguments( - {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/longitudinal_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", - share_dir + "/param/test_nearest_search.param.yaml"}); + {"--ros-args", "--params-file", + lateral_share_dir + "/param/lateral_controller_defaults.param.yaml", "--params-file", + longitudinal_share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", + share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", + share_dir + "/param/test_nearest_search.param.yaml", "--params-file", + share_dir + "/param/trajectory_follower_node.param.yaml"}); + + return node_options; +} + +std::shared_ptr makeNode(const rclcpp::NodeOptions & node_options) +{ std::shared_ptr node = std::make_shared(node_options); // Enable all logging in the node @@ -73,11 +88,14 @@ std::shared_ptr makeNode() class ControllerTester { public: - explicit ControllerTester(FakeNodeFixture * _fnf) : fnf(_fnf) {} - - std::shared_ptr node = makeNode(); + explicit ControllerTester(FakeNodeFixture * _fnf, const rclcpp::NodeOptions & node_options) + : fnf(_fnf), node(makeNode(node_options)) + { + } FakeNodeFixture * fnf; + std::shared_ptr node; + AckermannControlCommand::SharedPtr cmd_msg; bool received_control_command = false; @@ -172,7 +190,8 @@ TrajectoryPoint make_traj_point(const double px, const double py, const float vx TEST_F(FakeNodeFixture, no_input) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); // No published data: expect a stopped command test_utils::waitForMessage( @@ -182,7 +201,8 @@ TEST_F(FakeNodeFixture, no_input) TEST_F(FakeNodeFixture, empty_trajectory) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); @@ -200,7 +220,8 @@ TEST_F(FakeNodeFixture, empty_trajectory) // lateral TEST_F(FakeNodeFixture, straight_trajectory) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -225,7 +246,8 @@ TEST_F(FakeNodeFixture, straight_trajectory) TEST_F(FakeNodeFixture, right_turn) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -251,7 +273,8 @@ TEST_F(FakeNodeFixture, right_turn) TEST_F(FakeNodeFixture, left_turn) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -277,7 +300,8 @@ TEST_F(FakeNodeFixture, left_turn) TEST_F(FakeNodeFixture, stopped) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_odom(); @@ -306,7 +330,8 @@ TEST_F(FakeNodeFixture, stopped) // longitudinal TEST_F(FakeNodeFixture, longitudinal_keep_velocity) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -338,7 +363,8 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) TEST_F(FakeNodeFixture, longitudinal_slow_down) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_acc(); @@ -372,7 +398,8 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) TEST_F(FakeNodeFixture, longitudinal_accelerate) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_steer(); @@ -406,7 +433,8 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) TEST_F(FakeNodeFixture, longitudinal_stopped) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_odom(); @@ -433,7 +461,8 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) TEST_F(FakeNodeFixture, longitudinal_reverse) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); @@ -459,7 +488,8 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) TEST_F(FakeNodeFixture, longitudinal_emergency) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_odom(); @@ -482,3 +512,62 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } + +TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) +{ + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_acc(); + + // steering_tire_angle has to be larger than the threshold to check convergence. + const double steering_tire_angle = -0.5; + tester.publish_steer_angle(steering_tire_angle); + + // Publish trajectory starting away from the current ego pose + Trajectory traj; + traj.header.stamp = tester.node->now(); + traj.header.frame_id = "map"; + traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Not keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) +{ + // set enable_keep_stopped_until_steer_convergence true + const auto node_options = makeNodeOptions(true); + ControllerTester tester(this, node_options); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_acc(); + + // steering_tire_angle has to be larger than the threshold to check convergence. + const double steering_tire_angle = -0.5; + tester.publish_steer_angle(steering_tire_angle); + + // Publish trajectory starting away from the current ego pose + Trajectory traj; + traj.header.stamp = tester.node->now(); + traj.header.frame_id = "map"; + traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); +} diff --git a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp similarity index 99% rename from control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp rename to control/trajectory_follower_node/test/test_lateral_controller_node.cpp index 2109de2ece069..0c0a1f84ab1c2 100644 --- a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp @@ -19,7 +19,7 @@ #include "rclcpp/time.hpp" #include "trajectory_follower_test_utils.hpp" -#include +#include #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -33,7 +33,7 @@ #include #include -using LateralController = autoware::motion::control::trajectory_follower_nodes::LateralController; +using LateralController = autoware::motion::control::trajectory_follower_node::LateralController; using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -47,7 +47,7 @@ const rclcpp::Duration one_second(1, 0); std::shared_ptr makeLateralNode() { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); rclcpp::NodeOptions node_options; node_options.arguments( {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", diff --git a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp similarity index 99% rename from control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp rename to control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp index 2704d0596b4e0..badafd5f2fd15 100644 --- a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp @@ -19,7 +19,7 @@ #include "rclcpp/time.hpp" #include "trajectory_follower_test_utils.hpp" -#include +#include #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -32,7 +32,7 @@ #include using LongitudinalController = - autoware::motion::control::trajectory_follower_nodes::LongitudinalController; + autoware::motion::control::trajectory_follower_node::LongitudinalController; using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -43,7 +43,7 @@ using FakeNodeFixture = autoware::tools::testing::FakeTestNode; std::shared_ptr makeLongitudinalNode() { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); rclcpp::NodeOptions node_options; node_options.arguments( {"--ros-args", "--params-file", diff --git a/control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp b/control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp similarity index 100% rename from control/trajectory_follower_nodes/test/trajectory_follower_test_utils.hpp rename to control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp diff --git a/control/trajectory_follower_nodes/design/trajectory_follower-design.md b/control/trajectory_follower_nodes/design/trajectory_follower-design.md deleted file mode 100644 index 574ac2934c955..0000000000000 --- a/control/trajectory_follower_nodes/design/trajectory_follower-design.md +++ /dev/null @@ -1,59 +0,0 @@ -# Trajectory Follower Nodes - -## Purpose - -Generate control commands to follow a given Trajectory. - -## Design - -This is a node of the functionalities implemented in [trajectory_follower](../../trajectory_follower/design/trajectory_follower-design.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. - -![Controller](media/Controller.drawio.svg) - -The process flow is as follows. - -1. Set input data to the lateral controller -2. Compute lateral commands. -3. Sync the result of the lateral control to the longitudinal controller. - - steer angle convergence -4. Set input data to the longitudinal controller -5. Compute longitudinal commands. -6. Sync the result of the longitudinal control to the longitudinal controller. - - velocity convergence(currently not used) - -Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` - -- lateral controller - - `keep_steer_control_until_converged` -- longitudinal controller - - `enable_keep_stopped_until_steer_convergence` - -### Inputs / Outputs / API - -#### Inputs - -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering - -#### Outputs - -- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. - -#### Parameter - -- `ctrl_period`: control commands publishing period -- `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. - 1. Both commands have been received. - 2. The last received commands are not older than defined by `timeout_thr_sec`. -- `lateral_controller_mode`: `mpc_follower` or `pure_pursuit` - - (currently there is only `PID` for longitudinal controller) - -## Debugging - -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. - -A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. - -In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 4876699351ffd..a4ac84f5b43e0 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -2,6 +2,9 @@ ros__parameters: update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 + use_emergency_handling: false + check_external_emergency_heartbeat: false + use_start_request: false external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 644e937e0718d..48e7288150b4e 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -26,6 +26,7 @@ + diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 36e12ab28a3a6..e4eadc1b69d8c 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -61,7 +61,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) this->create_publisher("output/turn_indicators_cmd", durable_qos); hazard_light_cmd_pub_ = this->create_publisher("output/hazard_lights_cmd", durable_qos); - gate_mode_pub_ = this->create_publisher("output/gate_mode", durable_qos); engage_pub_ = this->create_publisher("output/engage", durable_qos); pub_external_emergency_ = @@ -84,6 +83,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); mrm_state_sub_ = this->create_subscription( "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); + gear_status_sub_ = this->create_subscription( + "input/gear_status", 1, std::bind(&VehicleCmdGate::onGearStatus, this, _1)); // Subscriber for auto auto_control_cmd_sub_ = this->create_subscription( @@ -126,40 +127,40 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/emergency/gear_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyShiftCmd, this, _1)); // Parameter - update_period_ = 1.0 / declare_parameter("update_rate", 10.0); - use_emergency_handling_ = declare_parameter("use_emergency_handling", false); + update_period_ = 1.0 / declare_parameter("update_rate"); + use_emergency_handling_ = declare_parameter("use_emergency_handling"); check_external_emergency_heartbeat_ = - declare_parameter("check_external_emergency_heartbeat", false); + declare_parameter("check_external_emergency_heartbeat"); system_emergency_heartbeat_timeout_ = - declare_parameter("system_emergency_heartbeat_timeout", 0.5); + declare_parameter("system_emergency_heartbeat_timeout"); external_emergency_stop_heartbeat_timeout_ = - declare_parameter("external_emergency_stop_heartbeat_timeout", 0.5); - stop_hold_acceleration_ = declare_parameter("stop_hold_acceleration", -1.5); - emergency_acceleration_ = declare_parameter("emergency_acceleration", -2.4); + declare_parameter("external_emergency_stop_heartbeat_timeout"); + stop_hold_acceleration_ = declare_parameter("stop_hold_acceleration"); + emergency_acceleration_ = declare_parameter("emergency_acceleration"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; - p.vel_lim = declare_parameter("nominal.vel_lim", 25.0); - p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim", 5.0); - p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim", 5.0); - p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim", 5.0); - p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim", 5.0); - p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim", 1.0); + p.vel_lim = declare_parameter("nominal.vel_lim"); + p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim"); + p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim"); filter_.setParam(p); } { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; - p.vel_lim = declare_parameter("on_transition.vel_lim", 25.0); - p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim", 0.5); - p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim", 0.25); - p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim", 0.5); - p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim", 0.25); - p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim", 0.05); + p.vel_lim = declare_parameter("on_transition.vel_lim"); + p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim"); + p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim"); filter_on_transition_.setParam(p); } @@ -257,6 +258,8 @@ void VehicleCmdGate::onAutoHazardLightsCmd(HazardLightsCommand::ConstSharedPtr m void VehicleCmdGate::onAutoShiftCmd(GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; } +void VehicleCmdGate::onGearStatus(GearReport::ConstSharedPtr msg) { current_gear_ptr_ = msg; } + // for remote void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) { @@ -346,6 +349,15 @@ void VehicleCmdGate::onTimer() return; } + if (is_gate_mode_changed_) { + // If gate mode is external, is_engaged_ is always true + // While changing gate mode external to auto, the first is_engaged_ is always true for the first + // loop in this scope. So we need to wait for the second loop + // after gate mode is changed. + is_gate_mode_changed_ = false; + return; + } + // Select commands TurnIndicatorsCommand turn_indicator; HazardLightsCommand hazard_light; @@ -362,6 +374,11 @@ void VehicleCmdGate::onTimer() // Don't send turn signal when autoware is not engaged if (!is_engaged_) { + if (!current_gear_ptr_) { + gear.command = GearCommand::NONE; + } else { + gear.command = current_gear_ptr_.get()->report; + } turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND; hazard_light.command = HazardLightsCommand::NO_COMMAND; } @@ -564,7 +581,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) { const auto prev_gate_mode = current_gate_mode_; current_gate_mode_ = *msg; - + is_gate_mode_changed_ = true; if (current_gate_mode_.data != prev_gate_mode.data) { RCLCPP_INFO( get_logger(), "GateMode changed: %s -> %s", getGateModeName(prev_gate_mode.data), diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 756e8e122b13f..6358a039bbbfb 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::GearReport; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; @@ -100,18 +102,22 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Subscription::SharedPtr steer_sub_; rclcpp::Subscription::SharedPtr operation_mode_sub_; rclcpp::Subscription::SharedPtr mrm_state_sub_; + rclcpp::Subscription::SharedPtr gear_status_sub_; void onGateMode(GateMode::ConstSharedPtr msg); void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg); void onSteering(SteeringReport::ConstSharedPtr msg); void onMrmState(MrmState::ConstSharedPtr msg); + void onGearStatus(GearReport::ConstSharedPtr msg); bool is_engaged_; bool is_system_emergency_ = false; bool is_external_emergency_stop_ = false; + bool is_gate_mode_changed_ = false; double current_steer_ = 0; GateMode current_gate_mode_; MrmState current_mrm_state_; + GearReport::ConstSharedPtr current_gear_ptr_; // Heartbeat std::shared_ptr emergency_state_heartbeat_received_time_; diff --git a/launch/tier4_control_launch/CMakeLists.txt b/launch/tier4_control_launch/CMakeLists.txt index 06862431da1fd..1c4bccd83f619 100644 --- a/launch/tier4_control_launch/CMakeLists.txt +++ b/launch/tier4_control_launch/CMakeLists.txt @@ -6,6 +6,5 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE - config launch ) diff --git a/launch/tier4_control_launch/README.md b/launch/tier4_control_launch/README.md index 919258cfc60dc..2d2a6487986bd 100644 --- a/launch/tier4_control_launch/README.md +++ b/launch/tier4_control_launch/README.md @@ -12,9 +12,15 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `control.launch.py`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `planning.launch.xml`. + ```xml + + + + ... ``` diff --git a/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml b/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml deleted file mode 100644 index e3c78c90e2ed1..0000000000000 --- a/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - # Node - update_rate: 10.0 - - # Core - delay_time: 0.03 # delay time of vehicle [s] - footprint_margin: 0.0 # margin for footprint [m] - max_deceleration: 1.5 # max deceleration [m/ss] - resample_interval: 0.3 # interval distance to resample point cloud [m] - search_radius: 5.0 # search distance from trajectory to point cloud [m] diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml deleted file mode 100644 index a0ddef6e363cc..0000000000000 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - transition_timeout: 10.0 - frequency_hz: 10.0 - check_engage_condition: false # set false if you do not want to care about the engage condition. - engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. - dist_threshold: 1.5 - yaw_threshold: 0.524 - speed_upper_threshold: 10.0 - speed_lower_threshold: -10.0 - acc_threshold: 1.5 - lateral_acc_threshold: 1.0 - lateral_acc_diff_threshold: 0.5 - stable_check: - duration: 0.1 - dist_threshold: 1.5 - speed_upper_threshold: 2.0 - speed_lower_threshold: -2.0 - yaw_threshold: 0.262 diff --git a/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml b/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml deleted file mode 100644 index 4c45b36223401..0000000000000 --- a/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - park_on_goal: true diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml deleted file mode 100644 index 76b5da140bfaa..0000000000000 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ /dev/null @@ -1,74 +0,0 @@ -/**: - ros__parameters: - - # -- system -- - traj_resample_dist: 0.1 # path resampling interval [m] - use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value - - # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing - path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) - - # -- mpc optimization -- - qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) - mpc_prediction_horizon: 50 # prediction horizon step - mpc_prediction_dt: 0.1 # prediction horizon period [s] - mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q - mpc_weight_heading_error: 0.0 # heading error weight in matrix Q - mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q - mpc_weight_steering_input: 1.0 # steering error weight in matrix R - mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R - mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R - mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R - mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R - mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point - mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point - mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point - mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point - mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) - mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability - mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability - mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero - mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration - mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing - mpc_min_prediction_length: 5.0 # minimum prediction length - - # -- vehicle model -- - vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps: 40.0 # steering angle rate limit [deg/s] - acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] - velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] - - # -- lowpass filter for noise reduction -- - steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] - error_deriv_lpf_cutoff_hz: 5.0 - - # stop state: steering command is kept in the previous value in the stop state. - stop_state_entry_ego_speed: 0.001 - stop_state_entry_target_speed: 0.001 - converged_steer_rad: 0.1 - keep_steer_control_until_converged: true - new_traj_duration_time: 1.0 - new_traj_end_dist: 0.3 - - # vehicle parameters - vehicle: - cg_to_front_m: 1.228 - cg_to_rear_m: 1.5618 - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 diff --git a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml deleted file mode 100644 index 4876699351ffd..0000000000000 --- a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - update_rate: 10.0 - system_emergency_heartbeat_timeout: 0.5 - external_emergency_stop_heartbeat_timeout: 0.0 - stop_hold_acceleration: -1.5 - emergency_acceleration: -2.4 - stopped_state_entry_duration_time: 0.1 - nominal: - vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 - on_transition: - vel_lim: 50.0 - lon_acc_lim: 1.0 - lon_jerk_lim: 0.5 - lat_acc_lim: 1.2 - lat_jerk_lim: 0.75 diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 4cb4c54dd5ddb..7abb41d8af294 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -33,76 +31,38 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: + with open(LaunchConfiguration("vehicle_param_file").perform(context), "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - lat_controller_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "trajectory_follower", - "lateral_controller.param.yaml", - ) - with open(lat_controller_param_path, "r") as f: - lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] - lon_controller_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "trajectory_follower", - "longitudinal_controller.param.yaml", - ) - with open(lon_controller_param_path, "r") as f: + with open( + LaunchConfiguration("trajectory_follower_node_param_path").perform(context), "r" + ) as f: + trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lat_controller_param_path").perform(context), "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lon_controller_param_path").perform(context), "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - vehicle_cmd_gate_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "vehicle_cmd_gate", - "vehicle_cmd_gate.param.yaml", - ) - with open(vehicle_cmd_gate_param_path, "r") as f: + with open(LaunchConfiguration("vehicle_cmd_gate_param_path").perform(context), "r") as f: vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_departure_checker_param_path = LaunchConfiguration( - "lane_departure_checker_param_path" - ).perform(context) - with open(lane_departure_checker_param_path, "r") as f: + with open(LaunchConfiguration("lane_departure_checker_param_path").perform(context), "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - operation_mode_transition_manager_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "operation_mode_transition_manager", - "operation_mode_transition_manager.param.yaml", - ) - with open(operation_mode_transition_manager_param_path, "r") as f: + with open( + LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" + ) as f: operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - shift_decider_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "shift_decider", - "shift_decider.param.yaml", - ) - with open(shift_decider_param_path, "r") as f: + with open(LaunchConfiguration("shift_decider_param_path").perform(context), "r") as f: shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - obstacle_collision_checker_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "obstacle_collision_checker", - "obstacle_collision_checker.param.yaml", - ) - - with open(obstacle_collision_checker_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_collision_checker_param_path").perform(context), "r" + ) as f: obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + package="trajectory_follower_node", + plugin="autoware::motion::control::trajectory_follower_node::Controller", name="controller_node_exe", namespace="trajectory_follower", remappings=[ @@ -118,10 +78,10 @@ def launch_setup(context, *args, **kwargs): ], parameters=[ { - "ctrl_period": 0.03, "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), }, nearest_search_param, + trajectory_follower_node_param, lon_controller_param, lat_controller_param, vehicle_info_param, @@ -187,6 +147,7 @@ def launch_setup(context, *args, **kwargs): ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"), ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"), ("input/mrm_state", "/system/fail_safe/mrm_state"), + ("input/gear_status", "/vehicle/status/gear_status"), ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"), ("output/control_cmd", "/control/command/control_cmd"), ("output/gear_cmd", "/control/command/gear_cmd"), @@ -206,13 +167,6 @@ def launch_setup(context, *args, **kwargs): parameters=[ vehicle_cmd_gate_param, vehicle_info_param, - { - "use_emergency_handling": LaunchConfiguration("use_emergency_handling"), - "check_external_emergency_heartbeat": LaunchConfiguration( - "check_external_emergency_heartbeat" - ), - "use_start_request": LaunchConfiguration("use_start_request"), - }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -235,7 +189,7 @@ def launch_setup(context, *args, **kwargs): ("control_mode_request", "/control/control_mode_request"), ], parameters=[ - nearest_search_param_path, + nearest_search_param, operation_mode_transition_manager_param, vehicle_info_param, ], @@ -249,7 +203,10 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), ("target_container", "/control/control_container"), - ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + ( + "external_cmd_selector_param_path", + LaunchConfiguration("external_cmd_selector_param_path"), + ), ], ) @@ -328,58 +285,24 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) - # parameter - add_launch_arg( - "tier4_control_launch_param_path", - [ - FindPackageShare("tier4_control_launch"), - "/config", - ], - "tier4_control_launch parameter path", - ) - - # lateral controller - add_launch_arg( - "lateral_controller_mode", - "mpc_follower", - "lateral controller mode: `mpc_follower` or `pure_pursuit`", - ) - - # longitudinal controller mode - add_launch_arg( - "longitudinal_controller_mode", - "pid", - "longitudinal controller mode: `pid`", - ) - - add_launch_arg( - "vehicle_info_param_file", - [ - FindPackageShare("vehicle_info_util"), - "/config/vehicle_info.param.yaml", - ], - "path to the parameter file of vehicle information", - ) - - add_launch_arg( - "lane_departure_checker_param_path", - [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], - ) - - # obstacle collision checker - add_launch_arg("enable_obstacle_collision_checker", "false", "use obstacle collision checker") - - # velocity controller - add_launch_arg("show_debug_info", "false", "show debug information") - add_launch_arg("enable_pub_debug", "true", "enable to publish debug information") - - # vehicle cmd gate - add_launch_arg("use_emergency_handling", "false", "use emergency handling") - add_launch_arg("check_external_emergency_heartbeat", "true", "use external emergency stop") - add_launch_arg("use_start_request", "false", "use start request service") - - # external cmd selector - add_launch_arg("initial_selector_mode", "remote", "local or remote") + # option + add_launch_arg("vehicle_param_file") + add_launch_arg("vehicle_id") + add_launch_arg("enable_obstacle_collision_checker") + add_launch_arg("lateral_controller_mode") + add_launch_arg("longitudinal_controller_mode") + # common param path + add_launch_arg("nearest_search_param_path") + # package param path + add_launch_arg("trajectory_follower_node_param_path") + add_launch_arg("lat_controller_param_path") + add_launch_arg("lon_controller_param_path") + add_launch_arg("vehicle_cmd_gate_param_path") + add_launch_arg("lane_departure_checker_param_path") + add_launch_arg("operation_mode_transition_manager_param_path") + add_launch_arg("shift_decider_param_path") + add_launch_arg("obstacle_collision_checker_param_path") + add_launch_arg("external_cmd_selector_param_path") # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 900e1c55b48dc..bcb705d6b885e 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -15,8 +15,7 @@ external_cmd_selector lane_departure_checker shift_decider - trajectory_follower - trajectory_follower_nodes + trajectory_follower_node vehicle_cmd_gate ament_lint_auto diff --git a/launch/tier4_localization_launch/CMakeLists.txt b/launch/tier4_localization_launch/CMakeLists.txt index 056fe87ca3ab8..e327b2e955acd 100644 --- a/launch/tier4_localization_launch/CMakeLists.txt +++ b/launch/tier4_localization_launch/CMakeLists.txt @@ -6,5 +6,4 @@ autoware_package() ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_localization_launch/README.md b/launch/tier4_localization_launch/README.md index ee7d6cab09364..a95980d193e93 100644 --- a/launch/tier4_localization_launch/README.md +++ b/launch/tier4_localization_launch/README.md @@ -10,17 +10,15 @@ Please see `` in `package.xml`. ## Usage -You can include as follows in `*.launch.xml` to use `localization.launch.xml`. +Include `localization.launch.xml` in other launch files as follows. + +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `localization.launch.xml`. ```xml + + + + ... ``` - -## Notes - -There are some `param.yaml` files in `config` directory. - -```bash -ndt_scan_matcher.param.yaml -``` diff --git a/launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml b/launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml deleted file mode 100644 index ad5542315410e..0000000000000 --- a/launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - input_frame: "base_link" - output_frame: "base_link" - min_x: -60.0 - max_x: 60.0 - min_y: -60.0 - max_y: 60.0 - min_z: -30.0 - max_z: 50.0 - negative: False diff --git a/launch/tier4_localization_launch/config/localization_error_monitor.param.yaml b/launch/tier4_localization_launch/config/localization_error_monitor.param.yaml deleted file mode 100644 index 026daf0532d33..0000000000000 --- a/launch/tier4_localization_launch/config/localization_error_monitor.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - scale: 3.0 - error_ellipse_size: 1.0 - warn_ellipse_size: 0.8 - error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.2 diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml deleted file mode 100644 index 3b7f113c43a94..0000000000000 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ /dev/null @@ -1,67 +0,0 @@ -/**: - ros__parameters: - - # Vehicle reference frame - base_frame: "base_link" - - # Subscriber queue size - input_sensor_points_queue_size: 1 - - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 - - # The newton line search maximum step length - step_size: 0.1 - - # The ND voxel grid resolution - resolution: 2.0 - - # The number of iterations required to calculate alignment - max_iterations: 30 - - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 - - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 - - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 - - # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 - - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 - - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 - - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - - # Number of threads used for parallel computing - num_threads: 4 - - # The covariance of output pose - # Do note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] - - # Regularization switch - regularization_enabled: false - - # regularization scale factor - regularization_scale_factor: 0.01 diff --git a/launch/tier4_localization_launch/config/random_downsample_filter.param.yaml b/launch/tier4_localization_launch/config/random_downsample_filter.param.yaml deleted file mode 100644 index 53be849e0af22..0000000000000 --- a/launch/tier4_localization_launch/config/random_downsample_filter.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - sample_num: 1500 diff --git a/launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml b/launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml deleted file mode 100644 index 51a7ee9d89b6b..0000000000000 --- a/launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - voxel_size_x: 3.0 - voxel_size_y: 3.0 - voxel_size_z: 3.0 diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 2883a21877888..14fe9c55f7e5d 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,11 +1,16 @@ + + + + + + + - - @@ -16,15 +21,12 @@ - - - - + @@ -38,9 +40,7 @@ - - - + diff --git a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml index 43cfc7403184d..4caefed02584a 100644 --- a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml +++ b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml index 750adca4c2fa0..e4b04d3d4a32e 100644 --- a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml @@ -1,6 +1,5 @@ - @@ -10,7 +9,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index 44186b9986618..dd58311afe2df 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -20,7 +20,6 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare import yaml @@ -94,32 +93,24 @@ def add_launch_arg(name: str, default_value=None, description=None): arg = DeclareLaunchArgument(name, default_value=default_value, description=description) launch_arguments.append(arg) - add_launch_arg( - "tier4_localization_launch_param_path", - [FindPackageShare("tier4_localization_launch"), "/config"], - "tier4_localization_launch param path", - ) add_launch_arg( "crop_box_filter_measurement_range_param_path", [ - LaunchConfiguration("tier4_localization_launch_param_path"), - "/crop_box_filter_measurement_range.param.yaml", + LaunchConfiguration("crop_box_filter_measurement_range_param_path"), ], "path to the parameter file of crop_box_filter_measurement_range", ) add_launch_arg( "voxel_grid_downsample_filter_param_path", [ - LaunchConfiguration("tier4_localization_launch_param_path"), - "/voxel_grid_filter.param.yaml", + LaunchConfiguration("voxel_grid_downsample_filter_param_path"), ], "path to the parameter file of voxel_grid_downsample_filter", ) add_launch_arg( "random_downsample_filter_param_path", [ - LaunchConfiguration("tier4_localization_launch_param_path"), - "/random_downsample_filter.param.yaml", + LaunchConfiguration("random_downsample_filter_param_path"), ], "path to the parameter file of random_downsample_filter", ) diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index 3070bc80bbc54..1dfe2e3d5f109 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -2,7 +2,6 @@ - @@ -29,7 +28,6 @@ - diff --git a/launch/tier4_map_launch/README.md b/launch/tier4_map_launch/README.md index 344a56a0f9947..5e98593a9bec0 100644 --- a/launch/tier4_map_launch/README.md +++ b/launch/tier4_map_launch/README.md @@ -12,6 +12,8 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `map.launch.py`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `map.launch.xml`. + ```xml @@ -20,6 +22,11 @@ You can include as follows in `*.launch.xml` to use `map.launch.py`. + + + + + ... ``` diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index bf396921fcb7a..3be3082e58040 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -164,6 +164,7 @@ def add_launch_arg(name: str, default_value=None, description=None): [ FindPackageShare("tier4_map_launch"), "/config/pointcloud_map_loader.param.yaml", + # ToDo(kminoda): This file should eventually be removed as well as the other components ], "path to pointcloud_map_loader param file", ), diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 8fe3f141a0dbd..39a398e7c1d77 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,8 +1,10 @@ + + + - diff --git a/launch/tier4_perception_launch/CMakeLists.txt b/launch/tier4_perception_launch/CMakeLists.txt index 4962a2932201b..2474b2f4dd3af 100644 --- a/launch/tier4_perception_launch/CMakeLists.txt +++ b/launch/tier4_perception_launch/CMakeLists.txt @@ -7,5 +7,4 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_perception_launch/README.md b/launch/tier4_perception_launch/README.md index a81c43591483b..6877e66bef7f5 100644 --- a/launch/tier4_perception_launch/README.md +++ b/launch/tier4_perception_launch/README.md @@ -12,9 +12,16 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `perception.launch.xml`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `perception.launch.xml`. + ```xml + + + + + ... ``` diff --git a/launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml b/launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml deleted file mode 100644 index dfdee95642fed..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - filter_target_label: - UNKNOWN : true - CAR : false - TRUCK : false - BUS : false - TRAILER : false - MOTORCYCLE : false - BICYCLE : false - PEDESTRIAN : false diff --git a/launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml b/launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml deleted file mode 100644 index 70afd9d31be94..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**: - ros__parameters: - filter_target_label: - UNKNOWN : true - CAR : false - TRUCK : false - BUS : false - TRAILER : false - MOTORCYCLE : false - BICYCLE : false - PEDESTRIAN : false - - upper_bound_x: 100.0 - lower_bound_x: 0.0 - upper_bound_y: 10.0 - lower_bound_y: -10.0 diff --git a/launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml b/launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml deleted file mode 100644 index a07a9416c2a3e..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - use_down_sample_filter: False - down_sample_voxel_size: 0.1 - distance_threshold: 0.5 diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml deleted file mode 100644 index 2541ab0367cad..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ /dev/null @@ -1,66 +0,0 @@ -/**: - ros__parameters: - can_assign_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement - [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker - 0, 1, 1, 1, 1, 0, 0, 0, #CAR - 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK - 0, 1, 1, 1, 1, 0, 0, 0, #BUS - 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER - 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE - 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE - 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN - - max_dist_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER - 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE - 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE - 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN - max_area_matrix: - # NOTE: The size of truck is 12 m length x 3 m width. - # NOTE: The size of trailer is 20 m length x 3 m width. - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN - 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR - 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK - 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS - 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER - 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE - 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE - 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN - min_area_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN - 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR - 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK - 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS - 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER - 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE - 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE - 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN - max_rad_matrix: # If value is greater than pi, it will be ignored. - #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN - [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN - - min_iou_matrix: # If value is negative, it will be ignored. - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN - 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN diff --git a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml deleted file mode 100644 index 781ccb806b25d..0000000000000 --- a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml +++ /dev/null @@ -1,30 +0,0 @@ -pcl_grid_map_extraction: - num_processing_threads: 12 - cloud_transform: - translation: - x: 0.0 - y: 0.0 - z: 0.0 - rotation: # intrinsic rotation X-Y-Z (r-p-y)sequence - r: 0.0 - p: 0.0 - y: 0.0 - cluster_extraction: - cluster_tolerance: 0.2 - min_num_points: 3 - max_num_points: 1000000 - outlier_removal: - is_remove_outliers: false - mean_K: 10 - stddev_threshold: 1.0 - downsampling: - is_downsample_cloud: false - voxel_size: - x: 0.02 - y: 0.02 - z: 0.02 - grid_map: - min_num_points_per_cell: 3 - resolution: 0.3 - height_type: 1 - height_thresh: 1.0 diff --git a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml deleted file mode 100644 index 8d56e12244716..0000000000000 --- a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - additional_lidars: [] - ransac_input_topics: [] - use_single_frame_filter: False - use_time_series_filter: True - - common_crop_box_filter: - parameters: - min_x: -50.0 - max_x: 100.0 - min_y: -50.0 - max_y: 50.0 - max_z: 2.5 # recommended 2.5 for non elevation_grid_mode - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode - negative: False - - common_ground_filter: - plugin: "ground_segmentation::ScanGroundFilterComponent" - parameters: - global_slope_max_angle_deg: 10.0 - local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.2 - use_virtual_ground_point: True - split_height_distance: 0.2 - non_ground_height_threshold: 0.20 - grid_size_m: 0.1 - grid_mode_switch_radius: 20.0 - gnd_grid_buffer_size: 4 - detection_range_z_max: 2.5 - elevation_grid_mode: true diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 87a39cfb62c1a..db64703205a96 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,6 +1,5 @@ - @@ -21,16 +20,13 @@ - + - - - - @@ -248,6 +243,7 @@ + @@ -274,7 +270,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 40e9c407d38a2..9c0e2b1ac6d02 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,6 +1,5 @@ - @@ -64,7 +63,6 @@ - @@ -106,7 +104,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 1fb33b2be9014..665eb5f8a0ea7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -1,6 +1,5 @@ - @@ -11,14 +10,10 @@ - - - - @@ -143,7 +138,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index f6a88bb63f639..bd636b289ca93 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -29,8 +29,9 @@ class PointcloudMapFilterPipeline: def __init__(self, context): pointcloud_map_filter_param_path = os.path.join( - LaunchConfiguration("tier4_perception_launch_param_path").perform(context), - "object_recognition/detection/pointcloud_map_filter.param.yaml", + LaunchConfiguration( + "object_recognition_detection_pointcloud_map_filter_param_path" + ).perform(context), ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -150,7 +151,6 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") - add_launch_arg("tier4_perception_launch_param_path", "tier4_perception_launch parameter path") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 4ab68d11ea7c9..0d17e1d7338e5 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -2,11 +2,10 @@ - - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index b68275bdfe462..afc55582853f9 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -33,8 +33,9 @@ def __init__(self, context): self.context = context self.vehicle_info = self.get_vehicle_info() ground_segmentation_param_path = os.path.join( - LaunchConfiguration("tier4_perception_launch_param_path").perform(context), - "obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml", + LaunchConfiguration("obstacle_segmentation_ground_segmentation_param_path").perform( + context + ), ) with open(ground_segmentation_param_path, "r") as f: self.ground_segmentation_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -329,12 +330,9 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con "inpaint_radius": 1.0, "param_file_path": PathJoinSubstitution( [ - LaunchConfiguration("tier4_perception_launch_param_path").perform( - context - ), - "obstacle_segmentation", - "ground_segmentation", - "elevation_map_parameters.yaml", + LaunchConfiguration( + "obstacle_segmentation_ground_segmentation_elevation_map_param_path" + ).perform(context), ] ), "elevation_map_directory": PathJoinSubstitution( @@ -518,7 +516,6 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "perception_pipeline_container") - add_launch_arg("tier4_perception_launch_param_path", "tier4_perception_launch parameter path") add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b9a94a04e46da..995c71a831dd0 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,7 +1,17 @@ - - + + + + + + + + + + + + @@ -35,6 +45,7 @@ + @@ -45,7 +56,6 @@ - @@ -76,7 +86,6 @@ - @@ -106,9 +115,7 @@ - - - + @@ -127,7 +134,7 @@ - + diff --git a/launch/tier4_planning_launch/CMakeLists.txt b/launch/tier4_planning_launch/CMakeLists.txt index 306b557745756..040cc5a1ad0a6 100644 --- a/launch/tier4_planning_launch/CMakeLists.txt +++ b/launch/tier4_planning_launch/CMakeLists.txt @@ -7,5 +7,4 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_planning_launch/README.md b/launch/tier4_planning_launch/README.md index 2b5790536827c..392e324d71eb6 100644 --- a/launch/tier4_planning_launch/README.md +++ b/launch/tier4_planning_launch/README.md @@ -10,7 +10,13 @@ Please see `` in `package.xml`. ## Usage +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `planning.launch.xml`. + ```xml + + + + ... ``` diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml deleted file mode 100644 index 329714e3d371e..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml +++ /dev/null @@ -1,25 +0,0 @@ -/**: - ros__parameters: - resample: - ds_resample: 0.1 - num_resample: 1 - delta_yaw_threshold: 0.785 - - latacc: - enable_constant_velocity_while_turning: false - constant_velocity_dist_threshold: 2.0 - - forward: - max_acc: 1.0 - min_acc: -1.0 - max_jerk: 0.3 - min_jerk: -0.3 - kp: 0.3 - - backward: - start_jerk: -0.1 - min_jerk_mild_stop: -0.3 - min_jerk: -1.5 - min_acc_mild_stop: -1.0 - min_acc: -2.5 - span_jerk: -0.01 diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml deleted file mode 100644 index a6906b7117fd5..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - jerk_weight: 0.1 # weight for "smoothness" cost for jerk - over_v_weight: 10000.0 # weight for "over speed limit" cost - over_a_weight: 500.0 # weight for "over accel limit" cost - over_j_weight: 200.0 # weight for "over jerk limit" cost - jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml deleted file mode 100644 index c993978204f10..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - pseudo_jerk_weight: 100.0 # weight for "smoothness" cost - over_v_weight: 100000.0 # weight for "over speed limit" cost - over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml deleted file mode 100644 index ec38010621cc9..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - pseudo_jerk_weight: 200.0 # weight for "smoothness" cost - over_v_weight: 100000.0 # weight for "over speed limit" cost - over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml deleted file mode 100644 index 64b2d95c6fd01..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ /dev/null @@ -1,58 +0,0 @@ -/**: - ros__parameters: - # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] - stop_decel: 0.0 # deceleration at a stop point[m/ss] - - # external velocity limit parameter - margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] - - # curve parameters - max_lateral_accel: 1.0 # max lateral acceleration limit [m/ss] - min_curve_velocity: 0.5 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] - decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit - decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit - min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] - - # engage & replan parameters - replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] - engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) - engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) - engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. - stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] - - # stop velocity - stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] - stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. - - # path extraction parameters - extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] - extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] - delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] - - # resampling parameters for optimization - max_trajectory_length: 200.0 # max trajectory length for resampling [m] - min_trajectory_length: 150.0 # min trajectory length for resampling [m] - resample_time: 2.0 # resample total time for dense sampling [s] - dense_resample_dt: 0.2 # resample time interval for dense sampling [s] - dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] - sparse_resample_dt: 0.5 # resample time interval for sparse sampling [s] - sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] - - # resampling parameters for post process - post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] - post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] - post_resample_time: 10.0 # resample total time for dense sampling [s] - post_dense_resample_dt: 0.1 # resample time interval for dense sampling [s] - post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] - post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] - post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] - - # steering angle rate limit parameters - max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] - resample_ds: 0.1 # distance between trajectory points [m] - curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] - curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] - - # system - over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml deleted file mode 100644 index b4e992ab56916..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ /dev/null @@ -1,77 +0,0 @@ -# see AvoidanceParameters description in avoidance_module_data.hpp for description. -/**: - ros__parameters: - avoidance: - resample_interval_for_planning: 0.3 - resample_interval_for_output: 4.0 - detection_area_right_expand_dist: 0.0 - detection_area_left_expand_dist: 1.0 - - enable_avoidance_over_same_direction: true - enable_avoidance_over_opposite_direction: true - enable_update_path_when_object_is_gone: false - - # For target object filtering - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] - - threshold_distance_object_is_on_center: 1.0 # [m] - - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - - # For lateral margin - object_envelope_buffer: 0.3 # [m] - lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] - - prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] - - road_shoulder_safety_margin: 0.5 # [m] - - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - # For detection loss compensation - object_last_seen_threshold: 2.0 # [s] - - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] - - # bound clipping for objects - enable_bound_clipping: false - - # for debug - publish_debug_marker: false - print_debug_info: false - - # not enabled yet - longitudinal_collision_margin_min_distance: 0.0 # [m] - longitudinal_collision_margin_time: 0.0 - - - # avoidance is performed for the object type with true - target_object: - car: true - truck: true - bus: true - trailer: true - unknown: false - bicycle: false - motorcycle: false - pedestrian: false - - # ---------- advanced parameters ---------- - avoidance_execution_lateral_threshold: 0.499 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml deleted file mode 100644 index 6ed118001fa6a..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - backward_path_length: 5.0 - forward_path_length: 300.0 - backward_length_buffer_for_end_of_lane: 5.0 - backward_length_buffer_for_end_of_pull_over: 5.0 - backward_length_buffer_for_end_of_pull_out: 5.0 - minimum_lane_change_length: 12.0 - minimum_pull_over_length: 16.0 - drivable_area_resolution: 0.1 - drivable_lane_forward_length: 50.0 - drivable_lane_backward_length: 5.0 - drivable_lane_margin: 3.0 - drivable_area_margin: 6.0 - refine_goal_search_radius_range: 7.5 - turn_signal_intersection_search_distance: 30.0 - turn_signal_intersection_angle_threshold_deg: 15.0 - turn_signal_minimum_search_distance: 10.0 - turn_signal_search_time: 3.0 - turn_signal_shift_length_threshold: 0.3 - turn_signal_on_swerving: true - path_interval: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml deleted file mode 100644 index 660a4d2af0e84..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - avoidance: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - lane_change: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - lane_following: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - pull_out: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - pull_over: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - side_shift: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml deleted file mode 100644 index 1a66bc09d040c..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - lane_change: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_abort_lane_change: true - enable_collision_check_at_prepare_phase: true - use_predicted_path_outside_lanelet: true - use_all_predicted_path: true - enable_blocked_by_obstacle: false diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml deleted file mode 100644 index b6a9574bb4800..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - lane_following: - lane_change_prepare_duration: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml deleted file mode 100644 index 7a9a63f805d67..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml +++ /dev/null @@ -1,30 +0,0 @@ -/**: - ros__parameters: - pull_out: - th_arrived_distance: 1.0 - th_stopped_velocity: 0.01 - th_stopped_time: 1.0 - collision_check_margin: 1.0 - pull_out_finish_judge_buffer: 1.0 - # shift pull out - enable_shift_pull_out: true - shift_pull_out_velocity: 2.0 - pull_out_sampling_num: 4 - before_pull_out_straight_distance: 0.0 - minimum_shift_pull_out_distance: 20.0 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - # geometric pull out - enable_geometric_pull_out: true - geometric_pull_out_velocity: 1.0 - arc_path_interval: 1.0 - lane_departure_margin: 0.2 - backward_velocity: -1.0 - pull_out_max_steer_angle: 0.26 # 15deg - # search start pose backward - enable_back: true - search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 15.0 - backward_search_resolution: 2.0 - backward_path_update_duration: 3.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml deleted file mode 100644 index 44d859a477767..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ /dev/null @@ -1,61 +0,0 @@ -/**: - ros__parameters: - pull_over: - request_length: 200.0 - th_arrived_distance: 1.0 - th_stopped_velocity: 0.01 - th_stopped_time: 2.0 # It must be greater than the state_machine's. - pull_over_velocity: 3.0 - pull_over_minimum_velocity: 1.38 - margin_from_boundary: 0.5 - decide_path_distance: 10.0 - maximum_deceleration: 1.0 - # goal research - enable_goal_research: true - search_priority: "efficient_path" # "efficient_path" or "close_goal" - forward_goal_search_length: 20.0 - backward_goal_search_length: 20.0 - goal_search_interval: 2.0 - longitudinal_margin: 3.0 - # occupancy grid map - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false - occupancy_grid_collision_check_margin: 0.0 - theta_size: 360 - obstacle_threshold: 60 - # object recognition - use_object_recognition: true - object_recognition_collision_check_margin: 1.0 - # shift path - enable_shift_parking: true - pull_over_sampling_num: 4 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - after_pull_over_straight_distance: 5.0 - before_pull_over_straight_distance: 5.0 - # parallel parking path - enable_arc_forward_parking: true - enable_arc_backward_parking: true - after_forward_parking_straight_distance: 2.0 - after_backward_parking_straight_distance: 2.0 - forward_parking_velocity: 1.38 - backward_parking_velocity: -1.38 - forward_parking_lane_departure_margin: 0.0 - backward_parking_lane_departure_margin: 0.0 - arc_path_interval: 1.0 - pull_over_max_steer_angle: 0.35 # 20deg - # hazard on when parked - hazard_on_threshold_distance: 1.0 - hazard_on_threshold_velocity: 0.5 - # check safety with dynamic objects. Not used now. - pull_over_duration: 2.0 - pull_over_prepare_duration: 4.0 - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - # debug - print_debug_info: false diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml deleted file mode 100644 index 79044041b4889..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - side_shift: - min_distance_to_start_shifting: 5.0 - time_to_start_shifting: 1.0 - shifting_lateral_jerk: 0.2 - min_shifting_distance: 5.0 - min_shifting_speed: 5.56 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml deleted file mode 100644 index 1e0777ecf994a..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - launch_stop_line: true - launch_crosswalk: true - launch_traffic_light: true - launch_intersection: true - launch_blind_spot: true - launch_detection_area: true - launch_virtual_traffic_light: true - launch_occlusion_spot: true - launch_no_stopping_area: true - launch_run_out: false - forward_path_length: 1000.0 - backward_path_length: 5.0 - max_accel: -2.8 - max_jerk: -5.0 - system_delay: 0.5 - delay_response_time: 0.5 - is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml deleted file mode 100644 index 31f75d7f7c5df..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - blind_spot: - use_pass_judge_line: true - stop_line_margin: 1.0 # [m] - backward_length: 15.0 # [m] - ignore_width_from_center_line: 0.7 # [m] - max_future_movement_time: 10.0 # [second] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml deleted file mode 100644 index 2bbc5d31fcaff..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ /dev/null @@ -1,44 +0,0 @@ -/**: - ros__parameters: - crosswalk: - show_processing_time: false # [-] whether to show processing time - - # param for stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists - stop_line_margin: 10.0 # [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) - stop_margin: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin - stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk - - # param for ego velocity - min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) - max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake - max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake - no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) - - # param for stuck vehicle - stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck - max_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked - stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk - - # param for pass judge logic - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) - stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) - min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) - max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. - ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not - - # param for input data - tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - - # param for target area & object - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk - target_object: - unknown: true # [-] whether to look and stop by UNKNOWN objects - bicycle: true # [-] whether to look and stop by BICYCLE objects - motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) - pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects - - walkway: - stop_duration_sec: 1.0 # [s] stop time at stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml deleted file mode 100644 index 9174045bf0150..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - detection_area: - stop_margin: 0.0 - use_dead_line: false - dead_line_margin: 5.0 - use_pass_judge_line: false - state_clear_time: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml deleted file mode 100644 index 79eb257de6889..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ /dev/null @@ -1,26 +0,0 @@ -/**: - ros__parameters: - intersection: - state_transit_margin_time: 0.5 - stop_line_margin: 3.0 - keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr - keep_detection_vel_thr: 0.833 # == 3.0km/h - stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - intersection_velocity: 2.778 # 2.778m/s = 10.0km/h - intersection_max_accel: 0.5 # m/ss - detection_area_margin: 0.5 # [m] - detection_area_right_margin: 0.5 # [m] - detection_area_left_margin: 0.5 # [m] - detection_area_length: 200.0 # [m] - detection_area_angle_threshold: 0.785 # [rad] - min_predicted_path_confidence: 0.05 - collision_start_margin_time: 5.0 # [s] - collision_end_margin_time: 2.0 # [s] - use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck - assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning - enable_front_car_decel_prediction: false # By default this feature is disabled - - merge_from_private_road: - stop_duration_sec: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml deleted file mode 100644 index 32cd05a9cc153..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - no_stopping_area: - state_clear_time: 2.0 # [s] time to clear stop state - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - stop_margin: 0.0 # [m] margin to stop line at no stopping area - dead_line_margin: 1.0 # [m] dead line offset go at no stopping area - stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area - detection_area_length: 200.0 # [m] used to create detection area polygon - stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml deleted file mode 100644 index 957f7988a621b..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml +++ /dev/null @@ -1,34 +0,0 @@ -/**: - ros__parameters: - occlusion_spot: - detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" - pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" - use_object_info: true # [-] whether to reflect object info to occupancy grid map or not - use_moving_object_ray_cast: true # [-] whether to reflect moving object ray_cast to occupancy grid map or not - use_partition_lanelet: true # [-] whether to use partition lanelet map data - pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity - pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) - debug: # !Note: default should be false for performance - is_show_occlusion: false # [-] whether to show occlusion point markers. - is_show_cv_window: false # [-] whether to show open_cv debug window - is_show_processing_time: false # [-] whether to show processing time - threshold: - detection_area_length: 100.0 # [m] the length of path to consider perception range - stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop - lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision - motion: - safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety - max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake. - max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake. - non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. - non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. - min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed - safe_margin: 2.0 # [m] margin for detection failure(0.5m) + pedestrian radius(0.5m) + safe margin(1.0m) - detection_area: - min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. - min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper. - max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area. - grid: - free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid - occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml deleted file mode 100644 index ccf90a0b29794..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ /dev/null @@ -1,45 +0,0 @@ -/**: - ros__parameters: - run_out: - detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points - use_partition_lanelet: true # [-] whether to use partition lanelet map data - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates - stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin - passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin - deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles - detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles - detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time - min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length - - # parameter to create abstracted dynamic obstacles - dynamic_obstacle: - min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles - max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles - diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points - height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points - max_prediction_time: 10.0 # [sec] create predicted path until this time - time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path - points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - - # approach if ego has stopped in the front of the obstacle for a certain amount of time - approaching: - enable: false - margin: 0.0 # [m] distance on how close ego approaches the obstacle - limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - - # parameter to avoid sudden stopping - slow_down_limit: - enable: true - max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. - max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml deleted file mode 100644 index a2b5ac5d5fcd1..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - stop_line: - stop_margin: 0.0 - stop_check_dist: 2.0 - stop_duration_sec: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml deleted file mode 100644 index f5db276c9ead8..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - traffic_light: - stop_margin: 0.0 - tl_state_timeout: 1.0 - external_tl_state_timeout: 1.0 - yellow_lamp_period: 2.75 - enable_pass_judge: true diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml deleted file mode 100644 index 267dde50c7970..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - virtual_traffic_light: - max_delay_sec: 3.0 - near_line_distance: 1.0 - dead_line_margin: 1.0 - max_yaw_deviation_deg: 90.0 - check_timeout_after_stop_line: true diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml deleted file mode 100644 index 0bf42387a6621..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - backward_path_length: 5.0 - forward_path_length: 300.0 - max_accel: -5.0 - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - backward_length_buffer_for_end_of_lane: 5.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_length: 12.0 - minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 - prediction_time_resolution: 0.5 - drivable_area_resolution: 0.1 - drivable_area_width: 100.0 - drivable_area_height: 50.0 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - refine_goal_search_radius_range: 7.5 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 8ba0a402ee861..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,29 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "pull_over" - - "pull_out" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "pull_over" - - "pull_out" diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml deleted file mode 100644 index a18e531806171..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ /dev/null @@ -1,159 +0,0 @@ -/**: - ros__parameters: - option: - # publish - is_publishing_debug_visualization_marker: true - is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid - - is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area - - # show - is_showing_debug_info: false - is_showing_calculation_time: false - - # other - enable_avoidance: false # enable avoidance function - enable_pre_smoothing: true # enable EB - skip_optimization: false # skip MPT and EB - reset_prev_optimization: false - - common: - # sampling - num_sampling_points: 100 # number of optimizing points - - # trajectory total/fixing length - trajectory_length: 300.0 # total trajectory length[m] - - forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] - forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] - - backward_fixing_distance: 5.0 # backward fixing length from base_link [m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] - - delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] - delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point - delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point - - num_fix_points_for_extending: 50 # number of fixing points when extending - max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] - - enable_clipping_fixed_traj: false - non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory - - object: # avoiding object - max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] - max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] - - avoiding_object_type: - unknown: true - car: true - truck: true - bus: true - bicycle: true - motorbike: true - pedestrian: true - animal: true - - # mpt param - mpt: - option: - steer_limit_constraint: true - fix_points_around_ego: true - plan_from_ego: true - max_plan_from_ego_length: 10.0 - visualize_sampling_num: 1 - enable_manual_warm_start: true - enable_warm_start: true # false - is_fixed_point_single: false - - common: - num_curvature_sampling_points: 5 # number of sampling points when calculating curvature - delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] - - # kinematics: - # If this parameter is commented out, the parameter is set as below by default. - # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` - # The 0.8 scale is adopted as it performed the best. - # optimization_center_offset: 2.3 # optimization center offset from base link - - # replanning & trimming trajectory param outside algorithm - replan: - max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] - max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] - max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] - - # advanced parameters to improve performance as much as possible - advanced: - eb: - common: - num_joint_buffer_points: 3 # number of joint buffer points - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. - num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points - - clearance: - clearance_for_straight_line: 0.05 # minimum optimizing range around straight points - clearance_for_joint: 0.1 # minimum optimizing range around joint points - clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing - - qp: - max_iteration: 10000 # max iteration when solving QP - eps_abs: 1.0e-8 # eps abs when solving OSQP - eps_rel: 1.0e-10 # eps rel when solving OSQP - - mpt: - bounds_search_widths: [0.45, 0.15, 0.05, 0.01] - - clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory - hard_clearance_from_road: 0.0 # clearance from road boundary[m] - soft_clearance_from_road: 0.1 # clearance from road boundary[m] - soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road - - weight: - soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point - soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point - - lat_error_weight: 100.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - yaw_error_rate_weight: 0.0 # weight for yaw error rate - steer_input_weight: 10.0 # weight for steering input - steer_rate_weight: 10.0 # weight for steering rate - - obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error - obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error - obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error - near_objects_length: 30.0 # weight for yaw error - - terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point - terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point - terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point - terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point - - # check if planned trajectory is outside drivable area - collision_free_constraints: - option: - l_inf_norm: true - soft_constraint: true - hard_constraint: false - # two_step_soft_constraint: false - - vehicle_circles: - method: "rear_drive" - - uniform_circle: - num: 3 - radius_ratio: 0.8 - - rear_drive: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 - - bicycle_model: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml deleted file mode 100644 index 520637abed775..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ /dev/null @@ -1,115 +0,0 @@ -/**: - ros__parameters: - common: - planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - - is_showing_debug_info: true - - # longitudinal info - idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] - min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] - min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] - safe_distance_margin : 6.0 # This is also used as a stop margin [m] - terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] - - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index - nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index - min_behavior_stop_margin: 3.0 # [m] - obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - - cruise_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: false - pedestrian: false - - stop_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: true - pedestrian: true - - obstacle_filtering: - rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] - detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion - decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking - - # if crossing vehicle is decided as target obstacles or not - crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] - crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop - collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] - - outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] - outside_obstacle_min_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] - max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego - - prediction_resampling_time_interval: 0.1 - prediction_resampling_time_horizon: 10.0 - - goal_extension_length: 5.0 # extension length for collision check around the goal - goal_extension_interval: 1.0 # extension interval for collision check around the goal - - stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle - - ignored_outside_obstacle_type: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: true - pedestrian: true - - pid_based_planner: - # use_predicted_obstacle_pose: false - - # PID gains to keep safe distance with the front vehicle - kp: 2.5 - ki: 0.0 - kd: 2.3 - - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] - - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] - - min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] - - lpf_cruise_gain: 0.2 - - optimization_based_planner: - dense_resampling_time_interval: 0.2 - sparse_resampling_time_interval: 2.0 - dense_time_horizon: 5.0 - max_time_horizon: 25.0 - velocity_margin: 0.2 #[m/s] - - # Parameters for safe distance - t_dangerous: 0.5 - - # For initial Motion - replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] - engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) - engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) - engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. - stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] - - # Weights for optimization - max_s_weight: 100.0 - max_v_weight: 1.0 - over_s_safety_weight: 1000000.0 - over_s_ideal_weight: 50.0 - over_v_weight: 500000.0 - over_a_weight: 5000.0 - over_j_weight: 10000.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml deleted file mode 100644 index dd76f2c4e540d..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml +++ /dev/null @@ -1,38 +0,0 @@ -/**: - ros__parameters: - adaptive_cruise_control: - # Adaptive Cruise Control (ACC) config - use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not - use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not - consider_obj_velocity: true # consider forward vehicle velocity to ACC or not - - # general parameter for ACC - obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] - obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] - emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] - emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] - min_dist_stop: 4.0 # minimum distance of emergency stop [m] - max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] - min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control - standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] - min_dist_standard: 4.0 # minimum distance in active cruise control [m] - obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] - margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] - - # pid parameter for ACC - p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] - p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] - d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] - d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] - - # parameter for object velocity estimation - object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] - object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] - valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] - valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] - valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] - valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] - thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] - lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity - use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) - rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml deleted file mode 100644 index 55a21d6cd7b20..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ /dev/null @@ -1,49 +0,0 @@ -/**: - ros__parameters: - hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] - lowpass_gain: 0.9 # gain parameter for low pass filter [-] - max_velocity: 20.0 # max velocity [m/s] - enable_slow_down: False # whether to use slow down planner [-] - - stop_planner: - # params for stop position - stop_position: - max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] - min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] - hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] - - # params for detection area - detection_area: - lateral_margin: 0.0 # margin of vehicle footprint [m] - step_length: 1.0 # step length for pointcloud search range [m] - extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] - - - slow_down_planner: - # params for slow down section - slow_down_section: - longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] - longitudinal_backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m] - longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] - min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] - - # params for detection area - detection_area: - lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] - - # params for velocity - target_velocity: - max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] - min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] - slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] - - # params for deceleration constraints (use this param if consider_constraints is True) - constraints: - jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss] - jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] - jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] - - # others - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] - velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] - acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml deleted file mode 100644 index c6a0c275db0b4..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml +++ /dev/null @@ -1,33 +0,0 @@ -/**: - ros__parameters: - min_ttc: 1.0 # [s] minimum time without collision when doing projection at each trajectory point - distance_buffer: 0.0 # [m] extra distance to add to a projection (in addition to the vehicle overhang) - min_adjusted_velocity: 2.5 # [m/s] minimum velocity that the module can set - max_deceleration: 2.0 # [m/s²] maximum deceleration caused by the adjusted velocity - - trajectory_preprocessing: - start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory - max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted - max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted - downsample_factor: 10 # factor by which to downsample the input trajectory for calculation - calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading - - simulation: - model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle" - distance_method: exact # distance calculation method. Either "exact" or "approximation". - # parameters used only with the bicycle model - steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection - nb_points: 5 # number of points representing the curved projections - - obstacles: - dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'. - ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored - ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored - occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles - dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection - dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module - static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles - - guard_rail - filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles - rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection - rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml deleted file mode 100644 index 6aa4e71774742..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml +++ /dev/null @@ -1,15 +0,0 @@ -/**: - ros__parameters: - - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] - state_clear_time: 2.0 - - # ego stop state - stop_state_ego_speed: 0.1 #[m/s] - - # debug - publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets diff --git a/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml deleted file mode 100644 index d6152c4f56243..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml +++ /dev/null @@ -1,39 +0,0 @@ -/**: - ros__parameters: - # -- Node Configurations -- - planning_algorithm: "astar" - waypoints_velocity: 5.0 - update_rate: 10.0 - th_arrived_distance_m: 1.0 - th_stopped_time_sec: 1.0 - th_stopped_velocity_mps: 0.01 - th_course_out_distance_m: 1.0 - vehicle_shape_margin_m: 1.0 - replan_when_obstacle_found: true - replan_when_course_out: true - - # -- Configurations common to the all planners -- - # base configs - time_limit: 30000.0 - # robot configs # TODO replace by vehicle_info - robot_length: 4.5 - robot_width: 1.75 - robot_base2back: 1.0 - minimum_turning_radius: 9.0 - maximum_turning_radius: 9.0 - turning_radius_size: 1 - # search configs - theta_size: 144 - angle_goal_range: 6.0 - curve_weight: 1.2 - reverse_weight: 2.0 - lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 - # costmap configs - obstacle_threshold: 100 - - # -- A* search Configurations -- - astar: - only_behind_solutions: false - use_back: true - distance_heuristic_weight: 1.0 diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e179d410e345e..e628c120a0e36 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -1,14 +1,54 @@ - + + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22,10 +62,9 @@ - + - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 7cd07bdc47c1d..ffe3a32b6a1af 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,13 +1,4 @@ - - - - - - - - - @@ -16,18 +7,13 @@ - + - - @@ -38,8 +24,7 @@ - - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 25c23f9189087..1eccb82e4ae82 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -34,113 +32,30 @@ def launch_setup(context, *args, **kwargs): # vehicle information parameter - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: - vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context) + with open(vehicle_param_path, "r") as f: + vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"] # nearest search parameter - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] # behavior path planner - side_shift_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "side_shift", - "side_shift.param.yaml", - ) - with open(side_shift_param_path, "r") as f: + with open(LaunchConfiguration("side_shift_param_path").perform(context), "r") as f: side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - avoidance_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "avoidance", - "avoidance.param.yaml", - ) - with open(avoidance_param_path, "r") as f: + with open(LaunchConfiguration("avoidance_param_path").perform(context), "r") as f: avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_change_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "lane_change", - "lane_change.param.yaml", - ) - with open(lane_change_param_path, "r") as f: + with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f: lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_following_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "lane_following", - "lane_following.param.yaml", - ) - with open(lane_following_param_path, "r") as f: + with open(LaunchConfiguration("lane_following_param_path").perform(context), "r") as f: lane_following_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - pull_over_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "pull_over", - "pull_over.param.yaml", - ) - with open(pull_over_param_path, "r") as f: + with open(LaunchConfiguration("pull_over_param_path").perform(context), "r") as f: pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - pull_out_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "pull_out", - "pull_out.param.yaml", - ) - with open(pull_out_param_path, "r") as f: + with open(LaunchConfiguration("pull_out_param_path").perform(context), "r") as f: pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - drivable_area_expansion_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "drivable_area_expansion.param.yaml", - ) - with open(drivable_area_expansion_param_path, "r") as f: + with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - behavior_path_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "behavior_path_planner.param.yaml", - ) - with open(behavior_path_planner_param_path, "r") as f: + with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f: behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] behavior_path_planner_component = ComposableNode( @@ -169,7 +84,7 @@ def launch_setup(context, *args, **kwargs): pull_out_param, drivable_area_expansion_param, behavior_path_planner_param, - vehicle_info_param, + vehicle_param, { "bt_tree_config_path": [ FindPackageShare("behavior_path_planner"), @@ -182,155 +97,41 @@ def launch_setup(context, *args, **kwargs): ) # smoother param - common_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "common.param.yaml", - ) - with open(common_param_path, "r") as f: + with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: common_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - motion_velocity_smoother_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "motion_velocity_smoother", - "motion_velocity_smoother.param.yaml", - ) - with open(motion_velocity_smoother_param_path, "r") as f: + with open( + LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r" + ) as f: motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - smoother_type_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "motion_velocity_smoother", - "Analytical.param.yaml", - ) - with open(smoother_type_param_path, "r") as f: - smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open( + LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r" + ) as f: + behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] # behavior velocity planner - blind_spot_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "blind_spot.param.yaml", - ) - with open(blind_spot_param_path, "r") as f: + with open(LaunchConfiguration("blind_spot_param_path").perform(context), "r") as f: blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - crosswalk_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "crosswalk.param.yaml", - ) - with open(crosswalk_param_path, "r") as f: + with open(LaunchConfiguration("crosswalk_param_path").perform(context), "r") as f: crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - detection_area_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "detection_area.param.yaml", - ) - with open(detection_area_param_path, "r") as f: + with open(LaunchConfiguration("detection_area_param_path").perform(context), "r") as f: detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - intersection_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "intersection.param.yaml", - ) - with open(intersection_param_path, "r") as f: + with open(LaunchConfiguration("intersection_param_path").perform(context), "r") as f: intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - stop_line_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "stop_line.param.yaml", - ) - with open(stop_line_param_path, "r") as f: + with open(LaunchConfiguration("stop_line_param_path").perform(context), "r") as f: stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - traffic_light_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "traffic_light.param.yaml", - ) - with open(traffic_light_param_path, "r") as f: + with open(LaunchConfiguration("traffic_light_param_path").perform(context), "r") as f: traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - virtual_traffic_light_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "virtual_traffic_light.param.yaml", - ) - with open(virtual_traffic_light_param_path, "r") as f: + with open(LaunchConfiguration("virtual_traffic_light_param_path").perform(context), "r") as f: virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - occlusion_spot_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "occlusion_spot.param.yaml", - ) - with open(occlusion_spot_param_path, "r") as f: + with open(LaunchConfiguration("occlusion_spot_param_path").perform(context), "r") as f: occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - no_stopping_area_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "no_stopping_area.param.yaml", - ) - with open(no_stopping_area_param_path, "r") as f: + with open(LaunchConfiguration("no_stopping_area_param_path").perform(context), "r") as f: no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - run_out_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "run_out.param.yaml", - ) - with open(run_out_param_path, "r") as f: + with open(LaunchConfiguration("run_out_param_path").perform(context), "r") as f: run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - behavior_velocity_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "behavior_velocity_planner.param.yaml", - ) - with open(behavior_velocity_planner_param_path, "r") as f: + with open( + LaunchConfiguration("behavior_velocity_planner_param_path").perform(context), "r" + ) as f: behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] behavior_velocity_planner_component = ComposableNode( @@ -389,11 +190,11 @@ def launch_setup(context, *args, **kwargs): virtual_traffic_light_param, occlusion_spot_param, no_stopping_area_param, - vehicle_info_param, + vehicle_param, run_out_param, common_param, motion_velocity_smoother_param, - smoother_type_param, + behavior_velocity_smoother_type_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -477,7 +278,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg( - "vehicle_info_param_file", + "vehicle_param_file", [ FindPackageShare("vehicle_info_util"), "/config/vehicle_info.param.yaml", @@ -490,8 +291,6 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map") - add_launch_arg("tier4_planning_launch_param_path", None, "tier4_planning_launch parameter path") - # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index c4c7b3bab7597..92795bd2a9533 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -1,7 +1,7 @@ - + @@ -17,13 +17,13 @@ - + - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 9ea5200a1cbd2..e7e1292f1d2c1 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -32,40 +30,22 @@ def launch_setup(context, *args, **kwargs): # vehicle information param path - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] # planning common param path - common_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "common.param.yaml", - ) - with open(common_param_path, "r") as f: + with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: common_param = yaml.safe_load(f)["/**"]["ros__parameters"] # nearest search parameter - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] # obstacle avoidance planner - obstacle_avoidance_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_avoidance_planner", - "obstacle_avoidance_planner.param.yaml", - ) - with open(obstacle_avoidance_planner_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_avoidance_planner_param_path").perform(context), "r" + ) as f: obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_avoidance_planner_component = ComposableNode( package="obstacle_avoidance_planner", @@ -88,15 +68,9 @@ def launch_setup(context, *args, **kwargs): ) # obstacle velocity limiter - obstacle_velocity_limiter_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_velocity_limiter", - "obstacle_velocity_limiter.param.yaml", - ) - with open(obstacle_velocity_limiter_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_velocity_limiter_param_path").perform(context), "r" + ) as f: obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_velocity_limiter_component = ComposableNode( package="obstacle_velocity_limiter", @@ -122,15 +96,9 @@ def launch_setup(context, *args, **kwargs): ) # surround obstacle checker - surround_obstacle_checker_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "surround_obstacle_checker", - "surround_obstacle_checker.param.yaml", - ) - with open(surround_obstacle_checker_param_path, "r") as f: + with open( + LaunchConfiguration("surround_obstacle_checker_param_path").perform(context), "r" + ) as f: surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] surround_obstacle_checker_component = ComposableNode( package="surround_obstacle_checker", @@ -160,25 +128,11 @@ def launch_setup(context, *args, **kwargs): ) # obstacle stop planner - obstacle_stop_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_stop_planner", - "obstacle_stop_planner.param.yaml", - ) - obstacle_stop_planner_acc_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_stop_planner", - "adaptive_cruise_control.param.yaml", - ) - with open(obstacle_stop_planner_param_path, "r") as f: + with open(LaunchConfiguration("obstacle_stop_planner_param_path").perform(context), "r") as f: obstacle_stop_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(obstacle_stop_planner_acc_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_stop_planner_acc_param_path").perform(context), "r" + ) as f: obstacle_stop_planner_acc_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_stop_planner_component = ComposableNode( package="obstacle_stop_planner", @@ -214,15 +168,7 @@ def launch_setup(context, *args, **kwargs): ) # obstacle cruise planner - obstacle_cruise_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_cruise_planner", - "obstacle_cruise_planner.param.yaml", - ) - with open(obstacle_cruise_planner_param_path, "r") as f: + with open(LaunchConfiguration("obstacle_cruise_planner_param_path").perform(context), "r") as f: obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_cruise_planner_component = ComposableNode( package="obstacle_cruise_planner", @@ -317,7 +263,7 @@ def add_launch_arg(name: str, default_value=None, description=None): # vehicle information parameter file add_launch_arg( - "vehicle_info_param_file", + "vehicle_param_file", [ FindPackageShare("vehicle_info_util"), "/config/vehicle_info.param.yaml", @@ -338,8 +284,6 @@ def add_launch_arg(name: str, default_value=None, description=None): "cruise_planner", "obstacle_stop_planner", "cruise planner type" ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" - add_launch_arg("tier4_planning_launch_param_path", None, "tier4_planning_launch parameter path") - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 3d195185d9bd6..3f7650486e69d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -1,7 +1,7 @@ - + @@ -12,7 +12,7 @@ - + @@ -20,7 +20,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index 81758eec72485..b6400b568837a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -31,18 +29,11 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - freespace_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "parking", - "freespace_planner", - "freespace_planner.param.yaml", - ) - with open(freespace_planner_param_path, "r") as f: + with open(LaunchConfiguration("freespace_planner_param_path").perform(context), "r") as f: freespace_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] container = ComposableNodeContainer( @@ -136,7 +127,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg( - "vehicle_info_param_file", + "vehicle_param_file", [ FindPackageShare("vehicle_info_util"), "/config/vehicle_info.param.yaml", diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 6e3a183220d0a..741df95937c42 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -1,5 +1,5 @@ - + @@ -12,7 +12,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 1bcc85f6cf33b..103c2172ec7f0 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,14 +1,4 @@ - - - - - - - - - - @@ -25,25 +15,23 @@ - - + - - - + + @@ -54,16 +42,15 @@ - + - - + diff --git a/launch/tier4_simulator_launch/CMakeLists.txt b/launch/tier4_simulator_launch/CMakeLists.txt index 6817e31c4876e..8a670fdefa19c 100644 --- a/launch/tier4_simulator_launch/CMakeLists.txt +++ b/launch/tier4_simulator_launch/CMakeLists.txt @@ -5,6 +5,5 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_package(INSTALL_TO_SHARE - config launch ) diff --git a/launch/tier4_simulator_launch/config/fault_injection.param.yaml b/launch/tier4_simulator_launch/config/fault_injection.param.yaml deleted file mode 100644 index 1a57b852f7361..0000000000000 --- a/launch/tier4_simulator_launch/config/fault_injection.param.yaml +++ /dev/null @@ -1,37 +0,0 @@ -/**: - ros__parameters: - event_diag_list: - vehicle_is_out_of_lane: "lane_departure" - trajectory_deviation_is_high: "trajectory_deviation" - localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" - map_version_is_different: "map_version" - trajectory_is_invalid: "trajectory_point_validation" - cpu_temperature_is_high: "CPU Temperature" - cpu_usage_is_high: "CPU Usage" - cpu_is_in_thermal_throttling: "CPU Thermal Throttling" - storage_temperature_is_high: "HDD Temperature" - storage_usage_is_high: "HDD Usage" - network_usage_is_high: "Network Usage" - clock_error_is_large: "NTP Offset" - gpu_temperature_is_high: "GPU Temperature" - gpu_usage_is_high: "GPU Usage" - gpu_memory_usage_is_high: "GPU Memory Usage" - gpu_is_in_thermal_throttling: "GPU Thermal Throttling" - driving_recorder_storage_error: "driving_recorder" - debug_data_logger_storage_error: "bagpacker" - emergency_stop_operation: "emergency_stop_operation" - vehicle_error_occurred: "vehicle_errors" - vehicle_ecu_connection_is_lost: "can_bus_connection" - obstacle_crash_sensor_is_activated: "obstacle_crash" - /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat" - /control/autonomous_driving/node_alive_monitoring: "control_topic_status" - /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat" - /localization/node_alive_monitoring: "localization_topic_status" - /map/node_alive_monitoring: "map_topic_status" - /planning/node_alive_monitoring: "planning_topic_status" - /sensing/lidar/node_alive_monitoring: "lidar_topic_status" - /sensing/imu/node_alive_monitoring: "imu_connection" - /sensing/gnss/node_alive_monitoring: "gnss_connection" - /system/node_alive_monitoring: "system_topic_status" - /vehicle/node_alive_monitoring: "vehicle_topic_status" diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8705ba30a9350..d48485bc3ff06 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -1,6 +1,7 @@ - - + + + @@ -20,7 +21,7 @@ - + @@ -80,7 +81,7 @@ - + diff --git a/launch/tier4_system_launch/CMakeLists.txt b/launch/tier4_system_launch/CMakeLists.txt index 89170940dbab1..cde16af48f523 100644 --- a/launch/tier4_system_launch/CMakeLists.txt +++ b/launch/tier4_system_launch/CMakeLists.txt @@ -6,5 +6,4 @@ autoware_package() ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_system_launch/README.md b/launch/tier4_system_launch/README.md index 252701df8eaa3..76cea5a575c5d 100644 --- a/launch/tier4_system_launch/README.md +++ b/launch/tier4_system_launch/README.md @@ -10,10 +10,17 @@ Please see `` in `package.xml`. ## Usage +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `system.launch.xml`. + ```xml + + + + + ... ``` diff --git a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml deleted file mode 100644 index 5a5f2e880bcae..0000000000000 --- a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml +++ /dev/null @@ -1,182 +0,0 @@ -- module: map - mode: [online, planning_simulation] - type: launch - args: - node_name_suffix: vector_map - topic: /map/vector_map - topic_type: autoware_auto_mapping_msgs/msg/HADMapBin - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: map - mode: [online] - type: launch - args: - node_name_suffix: pointcloud_map - topic: /map/pointcloud_map - topic_type: sensor_msgs/msg/PointCloud2 - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: localization - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: initialpose3d - topic: /initialpose3d - topic_type: geometry_msgs/msg/PoseWithCovarianceStamped - best_effort: false - transient_local: false - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: localization - mode: [online] - type: autonomous - args: - node_name_suffix: pose_twist_fusion_filter_pose - topic: /localization/pose_twist_fusion_filter/pose - topic_type: geometry_msgs/msg/PoseStamped - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: perception - mode: [online] - type: launch - args: - node_name_suffix: obstacle_segmentation_pointcloud - topic: /perception/obstacle_segmentation/pointcloud - topic_type: sensor_msgs/msg/PointCloud2 - best_effort: true - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: perception - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: object_recognition_objects - topic: /perception/object_recognition/objects - topic_type: autoware_auto_perception_msgs/msg/PredictedObjects - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: planning - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: mission_planning_route - topic: /planning/mission_planning/route - topic_type: autoware_planning_msgs/msg/LaneletRoute - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: planning - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: scenario_planning_trajectory - topic: /planning/scenario_planning/trajectory - topic_type: autoware_auto_planning_msgs/msg/Trajectory - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: control - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: trajectory_follower_control_cmd - topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: control - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: control_command_control_cmd - topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: vehicle - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: vehicle_status_velocity_status - topic: /vehicle/status/velocity_status - topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: vehicle - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: vehicle_status_steering_status - topic: /vehicle/status/steering_status - topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: system - mode: [online, planning_simulation] - type: launch - args: - node_name_suffix: system_emergency_control_cmd - topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: localization - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: transform_map_to_base_link - topic: /tf - frame_id: map - child_frame_id: base_link - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml b/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml deleted file mode 100644 index 652a984ce539a..0000000000000 --- a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# Default configuration for emergency handler ---- -/**: - ros__parameters: - update_rate: 10 - timeout_hazard_status: 0.5 - timeout_takeover_request: 10.0 - use_takeover_request: false - use_parking_after_stopped: false - use_comfortable_stop: false - - # setting whether to turn hazard lamp on for each situation - turning_hazard_on: - emergency: true diff --git a/launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml b/launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml deleted file mode 100644 index 81bc9b9c0b5d0..0000000000000 --- a/launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - update_rate: 10 - min_acceleration: -1.0 - max_jerk: 0.3 - min_jerk: -0.3 diff --git a/launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml b/launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml deleted file mode 100644 index 1ee2699a23a82..0000000000000 --- a/launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - update_rate: 30 - target_acceleration: -2.5 - target_jerk: -1.5 diff --git a/launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml deleted file mode 100644 index e96e3b3b05934..0000000000000 --- a/launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - vehicle: - type: diagnostic_aggregator/AnalyzerGroup - path: vehicle - analyzers: - vehicle_errors: - type: diagnostic_aggregator/GenericAnalyzer - path: vehicle_errors - contains: [": vehicle_errors"] - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml deleted file mode 100644 index b9de5cc4f2e13..0000000000000 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml +++ /dev/null @@ -1,50 +0,0 @@ -# Description: -# name: diag name -# sf_at: diag level where it becomes Safe Fault -# lf_at: diag level where it becomes Latent Fault -# spf_at: diag level where it becomes Single Point Fault -# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# -# Note: -# empty-value for sf_at, lf_at and spf_at is "none" -# default values are: -# sf_at: "none" -# lf_at: "warn" -# spf_at: "error" -# auto_recovery: "true" ---- -/**: - ros__parameters: - required_modules: - autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default - /autoware/control/control_command_gate/node_alive_monitoring: default - - /autoware/localization/node_alive_monitoring: default - /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_accuracy: default - - /autoware/map/node_alive_monitoring: default - - /autoware/perception/node_alive_monitoring: default - - /autoware/planning/node_alive_monitoring: default - /autoware/planning/performance_monitoring/trajectory_validation: default - - # /autoware/sensing/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default - - external_control: - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/external_control/external_command_selector/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - - /autoware/vehicle/node_alive_monitoring: default diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml deleted file mode 100644 index bf40c334f6498..0000000000000 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,50 +0,0 @@ -# Description: -# name: diag name -# sf_at: diag level where it becomes Safe Fault -# lf_at: diag level where it becomes Latent Fault -# spf_at: diag level where it becomes Single Point Fault -# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# -# Note: -# empty-value for sf_at, lf_at and spf_at is "none" -# default values are: -# sf_at: "none" -# lf_at: "warn" -# spf_at: "error" -# auto_recovery: "true" ---- -/**: - ros__parameters: - required_modules: - autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default - /autoware/control/control_command_gate/node_alive_monitoring: default - - /autoware/localization/node_alive_monitoring: default - # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_accuracy: default - - /autoware/map/node_alive_monitoring: default - - /autoware/perception/node_alive_monitoring: default - - /autoware/planning/node_alive_monitoring: default - /autoware/planning/performance_monitoring/trajectory_validation: default - - # /autoware/sensing/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default - - external_control: - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/external_control/external_command_selector/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - - /autoware/vehicle/node_alive_monitoring: default diff --git a/launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml deleted file mode 100644 index da4d74e53d475..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - usage_warn: 0.96 - usage_error: 0.96 - usage_warn_count: 1 - usage_error_count: 2 - usage_avg: true - msr_reader_port: 7634 diff --git a/launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml deleted file mode 100644 index d96b9f24640b2..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - temp_warn: 90.0 - temp_error: 95.0 - gpu_usage_warn: 0.90 - gpu_usage_error: 1.00 - memory_usage_warn: 0.95 - memory_usage_error: 0.99 diff --git a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml deleted file mode 100644 index d818d848be801..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml +++ /dev/null @@ -1,23 +0,0 @@ -/**: - ros__parameters: - hdd_reader_port: 7635 - num_disks: 1 - disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1} - disk0: - name: / - temp_attribute_id: 0xC2 - temp_warn: 55.0 - temp_error: 70.0 - power_on_hours_attribute_id: 0x09 - power_on_hours_warn: 3000000 - total_data_written_attribute_id: 0xF1 - total_data_written_warn: 4915200 # =150TB (1unit=32MB) - total_data_written_safety_factor: 0.05 - recovered_error_attribute_id: 0xC3 - recovered_error_warn: 1 - free_warn: 5120 # MB(8hour) - free_error: 100 # MB(last 1 minute) - read_data_rate_warn: 360.0 # MB/s - write_data_rate_warn: 103.5 # MB/s - read_iops_warn: 63360.0 # IOPS - write_iops_warn: 24120.0 # IOPS diff --git a/launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml deleted file mode 100644 index f2da3972be91a..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - available_size: 1024 # MB diff --git a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml deleted file mode 100644 index d72b8d1334c17..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - devices: ["*"] - traffic_reader_port: 7636 - monitor_program: "greengrass" - crc_error_check_duration: 1 - crc_error_count_threshold: 1 - reassembles_failed_check_duration: 1 - reassembles_failed_check_count: 1 diff --git a/launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml deleted file mode 100644 index db54f70d1ce59..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - server: ntp.nict.jp - offset_warn: 0.1 - offset_error: 5.0 diff --git a/launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml deleted file mode 100644 index 3d6d82fae5ce2..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - num_of_procs: 5 diff --git a/launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml deleted file mode 100644 index e55410be39250..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - cmos_battery_warn: 2.90 - cmos_battery_error: 2.70 - cmos_battery_label: "" diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index ff3ebae90e847..ae64cac3ad2f4 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -1,9 +1,25 @@ + + + + + + + + + + + + + + + + + - @@ -14,37 +30,38 @@ - - - - - - - - + + + + + + + + + + + + + - + - - + + - + @@ -52,21 +69,19 @@ - + - - + - - + diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index b4a3c7118e051..b413779705af9 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -80,6 +80,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( } else { twist_with_cov.header.stamp = latest_vehicle_twist_stamp; } + twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; twist_with_cov.twist.twist.linear.x = vx_mean; twist_with_cov.twist.twist.angular = gyro_mean; @@ -211,6 +212,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m sensor_msgs::msg::Imu gyro_base_link; gyro_base_link.header = imu_msg_ptr->header; + gyro_base_link.header.frame_id = output_frame_; gyro_base_link.angular_velocity = transformed_angular_velocity.vector; gyro_base_link.angular_velocity_covariance = transformCovariance(imu_msg_ptr->angular_velocity_covariance); diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 8931c62e30704..258e942169556 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -10,5 +10,6 @@ ament_auto_add_executable(localization_error_monitor ) ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index e94f86c8d6c7e..86b0382527201 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -22,3 +22,7 @@ 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, ] + + # use partial map loading for large maps + # Note: You also need to enable partial_map_loading interface in pointcloud_map_loader to use this functionality + enable_partial_map_load: false diff --git a/localization/pose_initializer/docs/map_height_fitter.md b/localization/pose_initializer/docs/map_height_fitter.md index ee7fdfb44264d..2715ab7f91ac3 100644 --- a/localization/pose_initializer/docs/map_height_fitter.md +++ b/localization/pose_initializer/docs/map_height_fitter.md @@ -7,8 +7,21 @@ Use this service as preprocessing for `pose_initializer` when using a initial po This service replaces the Z value of the input pose with the lowest point of the map point cloud within a cylinder of XY-radius. If no point is found in this range, returns the input pose without changes. +Note that this package supports partial map loading interface, which is disabled by default. The interface is intended to be enabled when +the pointcloud map is too large to handle. By using the interface, the node will request for a partial map around the requested position +instead of loading whole map by subscription interface. To use this interface, + +1. Set `enable_partial_map_load` in this node to `true` +2. Set `enable_partial_load` in `pointcloud_map_loader` to `true` + ## Interfaces +### Parameters + +| Name | Type | Description | | +| ------------------------- | ---- | ------------------------------------------------------------------------------------- | ----- | +| `enable_partial_map_load` | bool | If true, use partial map load interface. If false, use topic subscription interaface. | false | + ### Services | Name | Type | Description | @@ -20,3 +33,9 @@ If no point is found in this range, returns the input pose without changes. | Name | Type | Description | | --------------------- | ----------------------------- | -------------- | | `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map | + +### Clients + +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------- | -------------------------------------------- | +| `/map/get_partial_pointcloud_map` | autoware_map_msgs::srv::GetPartialPointCloudMap | client for requesting partial pointcloud map | diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 732457dfaac54..9a29fdd60fe02 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -5,8 +5,10 @@ - + + + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 2f39f74efa09c..48d3a203b8deb 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_map_msgs component_interface_specs component_interface_utils geometry_msgs diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp index 0e2bded754f61..44a9360d65d68 100644 --- a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp +++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp @@ -23,17 +23,33 @@ #endif #include +#include MapHeightFitter::MapHeightFitter() : Node("map_height_fitter"), tf2_listener_(tf2_buffer_) { + enable_partial_map_load_ = declare_parameter("enable_partial_map_load", false); + const auto durable_qos = rclcpp::QoS(1).transient_local(); using std::placeholders::_1; using std::placeholders::_2; - sub_map_ = create_subscription( - "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1)); srv_fit_ = create_service( "fit_map_height", std::bind(&MapHeightFitter::on_fit, this, _1, _2)); + + if (!enable_partial_map_load_) { + sub_map_ = create_subscription( + "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1)); + } else { + callback_group_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cli_get_partial_pcd_ = create_client( + "client_partial_map_load", rmw_qos_profile_default, callback_group_service_); + while (!cli_get_partial_pcd_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + this->get_logger(), + "Cannot find partial map loading interface. Please check the setting in " + "pointcloud_map_loader to see if the interface is enabled."); + } + } } void MapHeightFitter::on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) @@ -72,15 +88,62 @@ double MapHeightFitter::get_ground_height(const tf2::Vector3 & point) const return std::isfinite(height) ? height : point.getZ(); } +void MapHeightFitter::get_partial_point_cloud_map(const geometry_msgs::msg::Point & point) +{ + if (!cli_get_partial_pcd_) { + throw std::runtime_error{"Partial map loading in pointcloud_map_loader is not enabled"}; + } + const auto req = std::make_shared(); + req->area.center = point; + req->area.radius = 50; + + RCLCPP_INFO(this->get_logger(), "Send request to map_loader"); + auto res{cli_get_partial_pcd_->async_send_request( + req, [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = res.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(this->get_logger(), "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = res.wait_for(std::chrono::seconds(1)); + } + + RCLCPP_INFO( + this->get_logger(), "Loaded partial pcd map from map_loader (grid size: %d)", + static_cast(res.get()->new_pointcloud_with_ids.size())); + + sensor_msgs::msg::PointCloud2 pcd_msg; + for (const auto & pcd_with_id : res.get()->new_pointcloud_with_ids) { + if (pcd_msg.width == 0) { + pcd_msg = pcd_with_id.pointcloud; + } else { + pcd_msg.width += pcd_with_id.pointcloud.width; + pcd_msg.row_step += pcd_with_id.pointcloud.row_step; + pcd_msg.data.insert( + pcd_msg.data.end(), pcd_with_id.pointcloud.data.begin(), pcd_with_id.pointcloud.data.end()); + } + } + + map_frame_ = res.get()->header.frame_id; + map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + pcl::fromROSMsg(pcd_msg, *map_cloud_); +} + void MapHeightFitter::on_fit( const RequestHeightFitting::Request::SharedPtr req, - const RequestHeightFitting::Response::SharedPtr res) const + const RequestHeightFitting::Response::SharedPtr res) { const auto & position = req->pose_with_covariance.pose.pose.position; tf2::Vector3 point(position.x, position.y, position.z); std::string req_frame = req->pose_with_covariance.header.frame_id; res->success = false; + if (enable_partial_map_load_) { + get_partial_point_cloud_map(req->pose_with_covariance.pose.pose.position); + } + if (map_cloud_) { try { const auto stamped = tf2_buffer_.lookupTransform(map_frame_, req_frame, tf2::TimePointZero); diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp index 8845b77a78095..5efa97989b95f 100644 --- a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp +++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -40,12 +41,17 @@ class MapHeightFitter : public rclcpp::Node std::string map_frame_; pcl::PointCloud::Ptr map_cloud_; rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Client::SharedPtr cli_get_partial_pcd_; rclcpp::Service::SharedPtr srv_fit_; + bool enable_partial_map_load_; + rclcpp::CallbackGroup::SharedPtr callback_group_service_; + void on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void get_partial_point_cloud_map(const geometry_msgs::msg::Point & point); void on_fit( const RequestHeightFitting::Request::SharedPtr req, - const RequestHeightFitting::Response::SharedPtr res) const; + const RequestHeightFitting::Response::SharedPtr res); double get_ground_height(const tf2::Vector3 & point) const; }; diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp index 296a50e0cd3ab..ca4e75671243f 100644 --- a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp +++ b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp @@ -19,7 +19,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; auto node = std::make_shared(); executor.add_node(node); executor.spin(); diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp index abe1325eb1419..089381ab51c61 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp @@ -79,7 +79,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPreProcessor const int64_t input_channels; const int64_t input_width; const int64_t input_height; - const int64_t datatype_bytes; + const int64_t input_datatype_bytes; const std::shared_ptr feature_generator; TVMArrayContainer output; }; @@ -115,7 +115,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor const int64_t output_channels; const int64_t output_width; const int64_t output_height; - const int64_t datatype_bytes; + const int64_t output_datatype_bytes; const float32_t objectness_thresh_; const float32_t score_threshold_; const float32_t height_thresh_; diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 261e57cb894eb..49ecbae311b58 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -36,10 +36,10 @@ ApolloLidarSegmentationPreProcessor::ApolloLidarSegmentationPreProcessor( const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, bool8_t use_intensity_feature, bool8_t use_constant_feature, float32_t min_height, float32_t max_height) -: input_channels(config.network_inputs[0].second[1]), - input_width(config.network_inputs[0].second[2]), - input_height(config.network_inputs[0].second[3]), - datatype_bytes(config.tvm_dtype_bits / 8), +: input_channels(config.network_inputs[0].node_shape[1]), + input_width(config.network_inputs[0].node_shape[2]), + input_height(config.network_inputs[0].node_shape[3]), + input_datatype_bytes(config.network_inputs[0].tvm_dtype_bits / 8), feature_generator(std::make_shared( input_width, input_height, range, use_intensity_feature, use_constant_feature, min_height, max_height)) @@ -48,9 +48,9 @@ ApolloLidarSegmentationPreProcessor::ApolloLidarSegmentationPreProcessor( std::vector shape_x{1, input_channels, input_width, input_height}; TVMArrayContainer x{ shape_x, - config.tvm_dtype_code, - config.tvm_dtype_bits, - config.tvm_dtype_lanes, + config.network_inputs[0].tvm_dtype_code, + config.network_inputs[0].tvm_dtype_bits, + config.network_inputs[0].tvm_dtype_lanes, config.tvm_device_type, config.tvm_device_id}; output = x; @@ -68,7 +68,7 @@ TVMArrayContainerVector ApolloLidarSegmentationPreProcessor::schedule( TVMArrayCopyFromBytes( output.getArray(), feature_map_ptr->map_data.data(), - input_channels * input_height * input_width * datatype_bytes); + input_channels * input_height * input_width * input_datatype_bytes); return {output}; } @@ -78,10 +78,10 @@ ApolloLidarSegmentationPostProcessor::ApolloLidarSegmentationPostProcessor( const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, float32_t objectness_thresh, float32_t score_threshold, float32_t height_thresh, int32_t min_pts_num) -: output_channels(config.network_outputs[0].second[1]), - output_width(config.network_outputs[0].second[2]), - output_height(config.network_outputs[0].second[3]), - datatype_bytes(config.tvm_dtype_bits / 8), +: output_channels(config.network_outputs[0].node_shape[1]), + output_width(config.network_outputs[0].node_shape[2]), + output_height(config.network_outputs[0].node_shape[3]), + output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8), objectness_thresh_(objectness_thresh), score_threshold_(score_threshold), height_thresh_(height_thresh), @@ -100,7 +100,7 @@ std::shared_ptr ApolloLidarSegmentationPostProcessor std::vector feature(output_channels * output_width * output_height, 0); TVMArrayCopyToBytes( input[0].getArray(), feature.data(), - output_channels * output_width * output_height * datatype_bytes); + output_channels * output_width * output_height * output_datatype_bytes); cluster2d_->cluster( feature.data(), pc_ptr_, valid_idx, objectness_thresh_, true /*use all grids for clustering*/); auto object_array = cluster2d_->getObjects(score_threshold_, height_thresh_, min_pts_num_); diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 2591266652827..f2935ae98a4fa 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -4,6 +4,9 @@ project(lidar_centerpoint) find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(lidar_centerpoint): Remove once upgrading to TensorRT 8.5 is complete +add_compile_options(-Wno-deprecated-declarations) + option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) # set flags for CUDA availability @@ -107,12 +110,12 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) endfunction() # centerpoint - download(v1 pts_voxel_encoder_centerpoint.onnx 94dcf01ccd53ef14a79787afaada63b4) - download(v1 pts_backbone_neck_head_centerpoint.onnx 7046995e502e11f35c9459668d8afde3) + download(v2 pts_voxel_encoder_centerpoint.onnx e8369d7e0f59951cf87c0274830de1fe) + download(v2 pts_backbone_neck_head_centerpoint.onnx 44e44d26781a69c741bf9bddde57fbb3) # centerpoint_tiny - download(v1 pts_voxel_encoder_centerpoint_tiny.onnx 410f730c537968cb27fbd70c941849a8) - download(v1 pts_backbone_neck_head_centerpoint_tiny.onnx e97c165c7877222c0e27e44409a07517) + download(v2 pts_voxel_encoder_centerpoint_tiny.onnx 5fb774fac44dc83949d05eb3ba077309) + download(v2 pts_backbone_neck_head_centerpoint_tiny.onnx e4658325b70222f7c3637fe00e586b82) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 777b3fc191111..baaea06e59346 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -52,6 +52,12 @@ We trained the models using . You can download the onnx format of trained models by clicking on the links below. +- Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx) +- Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx) + +`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. +`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. + ## Standalone inference and visualization In addition to its use as a standard ROS node, `lidar_centerpoint` can also be used to perform inferences in an isolated manner. @@ -119,6 +125,10 @@ Example: [7] +[8] + +[9] + ## (Optional) Future extensions / Unimplemented parts ![path_shifter](./image/path_shifter.png) @@ -293,15 +273,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val ###### Parameters for shift parking -| Name | Unit | Type | Description | Default value | -| :--------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | -| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | -| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | -| before_pull_over_straight_distance | [m] | double | straight line distance before pull over end point. a safe path should have a distance that includes this | 5.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | ##### **geometric parallel parking** @@ -384,13 +363,13 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of #### General parameters for pull_out -| Name | Unit | Type | Description | Default value | -| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| pull_out_finish_judge_buffer | [m] | double | threshold for finish judgment distance from pull out end point | 1.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | #### **Safe check with obstacles in shoulder lane** @@ -418,15 +397,14 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral ###### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| before_pull_out_straight_distance | [m] | double | distance before pull out start point | 0.0 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | ##### **geometric pull out** diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index cefc482be4896..3a2bc1a0cd1c0 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -422,6 +422,7 @@ The avoidance specific parameter configuration file can be located at `src/autow | enable_avoidance_over_same_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | | enable_avoidance_over_opposite_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true | | enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_bound_clipping | `true` | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | (\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value. @@ -512,25 +513,3 @@ To print the debug message, just run the following ```bash ros2 topic echo /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array ``` - -### Visualizing drivable area boundary - -Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. - -For example, in the same area, one can perform avoidance and another cannot. This might be related to the drivable area issues due to the non-compliance vector map design from the user. - -To debug the issue, the drivable area boundary can be visualized. - -![drivable_area_boundary_marker1](./image/avoidance_design/drivable_area_boundary_marker_example1.png) - -![drivable_area_boundary_marker2](./image/avoidance_design/drivable_area_boundary_marker_example2.png) - -The boundary can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_boundary` - -### Visualizing drivable area - -The drivable area can be visualize by adding the drivable area plugin - -![drivable_area_plugin](./image/drivable_area_plugin.png) - -and then add `/planning/scenario_planning/lane_driving/behavior_planning/path` as the topic. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md new file mode 100644 index 0000000000000..0d17c8c9fd509 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -0,0 +1,92 @@ +# Drivable Area + +Drivable Area represents the area where ego vehicle can pass. + +## Purpose / Role + +In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. + +## Assumption + +Our drivable area has several assumptions. + +- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. + +- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). + +- Both left and right bounds should cover the front of the path and the end of the path. + +## Limitations + +Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. + +## Parameters for drivable area generation + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | +| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | +| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | + +Note that default values can be varied by the module. Please refer to the `config/drivable_area_expansion.yaml` file. + +## Inner-workings / Algorithms + +This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). + +### Drivable Lanes Generation + +Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Pull Over`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. + +```plantuml +struct DrivalbleLanes +{ + lanelet::ConstLanelet right_lanelet; // right most lane + lanelet::ConstLanelet left_lanelet; // left most lane + lanelet::ConstLanelets middle_lanelets; // middle lanes +}; +``` + +The image of the sorted drivable lanes is depicted in the following picture. + +![sorted_lanes](./image/drivable_area/sorted_lanes.drawio.svg) + +Note that, the order of drivable lanes become + +```plantuml +drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} +``` + +### Drivable Area Expansion + +Each module can expand the drivable area based on parameters. It expands right bound and left bound of target lanes. This enables large vehicles to pass narrow curve. The image of this process can be described as + +![expanded_lanes](./image/drivable_area/expanded_lanes.drawio.svg) + +Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. + +### Drivable Area Generation + +In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as + +```plantuml +std::vector left_bound; +std::vector right_bound; +``` + +and each point of right bound and left bound has a position in the absolute coordinate system. + +![drivable_lines](./image/drivable_area/drivable_lines.drawio.svg) + +### Visualizing maximum drivable area (Debug) + +Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. + +For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. + +To debug the issue, the maximum drivable area boundary can be visualized. + +![drivable_area_boundary_marker1](./image/drivable_area/drivable_area_boundary_marker_example1.png) + +![drivable_area_boundary_marker2](./image/drivable_area/drivable_area_boundary_marker_example2.png) + +The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` diff --git a/planning/behavior_path_planner/behavior_path_planner_lane_change.md b/planning/behavior_path_planner/behavior_path_planner_lane_change.md index ed23881cbc250..532383b488767 100644 --- a/planning/behavior_path_planner/behavior_path_planner_lane_change.md +++ b/planning/behavior_path_planner/behavior_path_planner_lane_change.md @@ -329,12 +329,9 @@ The following figure illustrate the intersection case. ![intersection](./image/lane_change/lane_change-intersection_case.png) -### Abort lane change - -The lane changing process can still be aborted if the proper conditions are satisfied. The following states are validated before the abort request is approved. +### Aborting lane change -1. Ego's current speed is under the threshold, and ego is still in the current lane -2. ego relative distance from the centerline and ego relative yaw from lanelet's orientation is under the threshold. +The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. The following depicts the flow of the abort lane change check. @@ -344,79 +341,116 @@ skinparam monochrome true skinparam defaultTextAlignment center title Abort Lane Change -start -if (path if safe?) then (**NO**) - if (current velocity < abort_lane_change_velocity_thresh && ego is in current lane ?) then (**TRUE**) - :**ABORT**; - stop - else (**FALSE**) - if (distance of ego to centerline < abort_lane_change_distance_thresh - && - ego's current yaw with reference to current lane's orientation < abort_lane_change_angle_thresh) then (**TRUE**) - :**ABORT**; - stop - else (**FALSE**) + +while(Lane Following) + if (Lane Change required) then (**YES**) + if (Safe to change lane) then (**SAFE**) + while(Lane Changing) + if (Lane Change Completed) then (**YES**) + break + else (**NO**) + if (Is Abort Condition Satisfied) then (**NO**) + else (**YES**) + if (Is Enough margin to retry lane change) then (**YES**) + if (Ego not depart from current lane yet) then (**YES**) + :Cancel lane change; + break + else (**NO**) + if (Can perform abort maneuver) then (**YES**) + :Perform abort maneuver; + break + else (NO) + :Stop or Cruise depending on the situation; + endif + endif + else (**NO**) + endif + endif endif + :Stop and wait; + endwhile + else (**UNSAFE**) endif -else (**YES**) -endif -:**DONT ABORT**; -stop + else (**NO**) + endif +endwhile +-[hidden]-> +detach @enduml ``` -## Parameters +#### Cancel -### Essential lane change parameters +Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver. -The following parameters are configurable in `behavior_path_planner.param.yaml`. +The function can be enabled by setting `enable_cancel_lane_change` to `true`. + +The following image illustrates the cancel process. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------- | ------------- | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 5.0 | -| `minimum_lane_change_length` | [m] | double | The minimum distance needed for changing lanes. | 12.0 | +![cancel](./image/lane_change/cancel_and_abort/lane_change-cancel.png) + +#### Abort + +Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_cancel_lane_change` and `enable_abort_lane_change` to `true`. The parameter `abort_max_lateral_jerk` need to be set to a high value in order for it to work. + +![abort](./image/lane_change/cancel_and_abort/lane_change-abort.png) + +#### Stop/Cruise + +The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. + +![stop](./image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png) + +## Parameters + +### Essential lane change parameters The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | ------- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `lane_changing_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 5.6 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | -| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | -| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | -| `lane_change_search_distance` | [m] | double | The turn signal activation distance during the lane change preparation phase. | 30.0 | -| `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | false | -| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | ------- | ------ | --------------------------------------------------------------------------------------- | ------------- | +| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | +| `minimum_lane_change_prepare_distance` | [m] | double | Minimum prepare distance for lane change | 2.0 | +| `minimum_lane_change_length` | [m] | double | The minimum distance needed for changing lanes. | 16.5 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `lane_changing_lateral_acc` | [m/s2] | double | Lateral acceleration value for lane change path generation | 0.5 | +| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | +| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | ### Collision checks during lane change The following parameters are configurable in `behavior_path_planner.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :------------------------------------ | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | -| `lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 5.0 | -| `longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | -| `expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*2) | -1.0 | -| `expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*2) | -1.0 | -| `rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | -| `rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | - -(\*2) the value must be negative. +| Name | Unit | Type | Description | Default value | +| :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | +| `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `use_predicted_path_outside_lanelet` | [-] | boolean | If true, include collision check for predicted path that is out of lanelet (freespace). | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + +(\*1) the value must be negative. ### Abort lane change The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | ----- | ------- | ------------------------------------------------------------------------------------ | ------------- | -| `enable_abort_lane_change` | [-] | boolean | Enable abort lane change. (\*1) | false | -| `abort_lane_change_velocity_thresh` | [m/s] | double | The velocity threshold to abort lane change while ego is still in current lane. | 0.5 | -| `abort_lane_change_angle_threshold` | [deg] | double | The angle threshold based on angle between ego's current yaw and lane's orientation. | 10.0 | -| `abort_lane_change_distance_threshold` | [m] | double | The distance threshold based on relative distance from ego pose to the centerline. | 0.3 | +| Name | Unit | Type | Description | Default value | +| :-------------------------- | ---- | ------- | --------------------------------------- | ------------- | +| `enable_cancel_lane_change` | [-] | boolean | Enable cancel lane change | true | +| `enable_abort_lane_change` | [-] | boolean | Enable abort lane change. | false | +| `abort_delta_time` | [s] | double | The time taken to start aborting. | 3.0 | +| `abort_max_lateral_jerk` | [s] | double | The maximum lateral jerk for abort path | 1000.0 | ### Debug diff --git a/planning/behavior_path_planner/behavior_path_planner_path_generation.md b/planning/behavior_path_planner/behavior_path_planner_path_generation.md new file mode 100644 index 0000000000000..da2ea34f1d673 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_path_generation.md @@ -0,0 +1,97 @@ +## Path Generation + +This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp). + +### Overview + +The base idea of the path generation in lane change and avoidance is to smoothly shift the reference path, such as the center line, in the lateral direction. This is achieved by using a constant-jerk profile as in the figure below. More details on how it is used can be found in [README](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/README.md). It is assumed that the reference path is smooth enough for this algorithm. + +The figure below explains how the application of a constant lateral jerk $l^{'''}(s)$ can be used to induce lateral shifting. In order to comply with the limits on lateral acceleration and velocity, zero-jerk time is employed in the figure ( $T_a$ and $T_v$ ). In each interval where constant jerk is applied, the shift position $l(s)$ can be characterized by a third-degree polynomial. Therefore the shift length from the reference path can then be calculated by combining spline curves. + +

+ +

+ +Note that, due to the rarity of the $T_v$ in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. + +### Mathematical Derivation + +By applying simple integral operations, the following analytical equations can be derived to describe the shift distance $l(t)$ at each time under lateral jerk, acceleration, and velocity constraints. + +```math +\begin{align} +l_1&= \frac{1}{6}jT_j^3\\[10pt] +l_2&= \frac{1}{6}j T_j^3 + \frac{1}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j\\[10pt] +l_3&= j T_j^3 + \frac{3}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j\\[10pt] +l_4&= j T_j^3 + \frac{3}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j + j(T_a + T_j)T_j T_v\\[10pt] +l_5&= \frac{11}{6} j T_j^3 + \frac{5}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j + j(T_a + T_j)T_j T_v \\[10pt] +l_6&= \frac{11}{6} j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v\\[10pt] +l_7&= 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v +\end{align} +``` + +These equations are used to determine the shape of a path. Additionally, by applying further mathematical operations to these basic equations, the expressions of the following subsections can be derived. + +#### Calculation of Maximum Acceleration from transition time and final shift length + +In the case where there are no limitations on lateral velocity and lateral acceleration, the maximum lateral acceleration during the shifting can be calculated as follows. The constant-jerk time is given by $T_j = T_{\rm total}/4$ because of its symmetric property. Since $T_a=T_v=0$, the final shift length $L=l_7=2jT_j^3$ can be determined using the above equation. The maximum lateral acceleration is then given by $a_{\rm max} =jT_j$. This results in the following expression for the maximum lateral acceleration: + +```math +\begin{align} +a_{\rm max} = \frac{8L}{T_{\rm total}^2} +\end{align} +``` + +#### Calculation of Ta, Tj and jerk from acceleration limit + +In the case where there are no limitations on lateral velocity, the constant-jerk and acceleration times, as well as the required jerk can be calculated from the acceleration limit, total time, and final shift length as follows. Since $T_v=0$, the final shift length is given by: + +```math +\begin{align} +L = l_7 = 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j +\end{align} +``` + +Additionally, the velocity profile reveals the following relations: + +```math +\begin{align} +a_{\rm lim} &= j T_j\\ +T_{\rm total} &= 4T_j + 2T_a +\end{align} +``` + +By solving these three equations, the following can be obtained: + +```math +\begin{align} +T_j&=\frac{T_{\rm total}}{2} - \frac{2L}{a_{\rm lim} T_{\rm total}}\\[10pt] +T_a&=\frac{4L}{a_{\rm lim} T_{\rm total}} - \frac{T_{\rm total}}{2}\\[10pt] +jerk&=\frac{2a_{\rm lim} ^2T_{\rm total}}{a_{\rm lim} T_{\rm total}^2-4L} +\end{align} +``` + +where $T_j$ is the constant-jerk time, $T_a$ is the constant acceleration time, $j$ is the required jerk, $a_{\rm lim}$ is the acceleration limit, and $L$ is the final shift length. + +#### Calculation of Required Time from Jerk and Acceleration Constraint + +In the case where there are no limitations on lateral velocity, the total time required for shifting can be calculated from the jerk and acceleration limits and the final shift length as follows. By solving the two equations given above: + +```math +L = l_7 = 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j,\quad a_{\rm lim} = j T_j +``` + +we obtain the following expressions: + +```math +\begin{align} +T_j &= \frac{a_{\rm lim}}{j}\\[10pt] +T_a &= \frac{1}{2}\sqrt{\frac{a_{\rm lim}}{j}^2 + \frac{4L}{a_{\rm lim}}} - \frac{3a_{\rm lim}}{2j} +\end{align} +``` + +The total time required for shifting can then be calculated as $T_{\rm total}=4T_j+2T_a$. + +### Limitation + +- Since $T_v$ is zero in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. diff --git a/planning/behavior_path_planner/behavior_path_planner_side-shift.md b/planning/behavior_path_planner/behavior_path_planner_side-shift.md new file mode 100644 index 0000000000000..22c55ddc7370b --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_side-shift.md @@ -0,0 +1,92 @@ +# Side shift Module + +(For remote control) Shift the path to left or right according to an external instruction. + +## Flowchart + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title callback function of lateral offset input +start + +partition onLateralOffset { +:**INPUT** double new_lateral_offset; + +if (abs(inserted_lateral_offset_ - new_lateral_offset) < 1e-4) then ( true) + stop +else ( false) + if (interval from last request is too short) then ( no) + else ( yes) + :requested_lateral_offset_ = new_lateral_offset \n lateral_offset_change_request_ = true; + endif +stop +@enduml +``` + +```plantuml --> +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title path generation + +start +partition plan { +if (lateral_offset_change_request_ == true \n && \n (shifting_status_ == BEFORE_SHIFT \n || \n shifting_status_ == AFTER_SHIFT)) then ( true) + partition replace-shift-line { + if ( shift line is inserted in the path ) then ( yes) + :erase left shift line; + else ( no) + endif + :calcShiftLines; + :add new shift lines; + :inserted_lateral_offset_ = requested_lateral_offset_ \n inserted_shift_lines_ = new_shift_line; + } +else( false) +endif +stop +@enduml +``` + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title update state + +start +partition updateState { + :last_sp = path_shifter_.getLastShiftLine(); + note left + get furthest shift lines + end note + :calculate max_planned_shift_length; + note left + calculate furthest shift length of previous shifted path + end note + if (abs(inserted_lateral_offset_ - inserted_shift_line_.end_shift_length) < 1e-4 \n && \n abs(max_planned_shift_length) < 1e-4 \n && \n abs(requested_lateral_offset_) < 1e-4) then ( true) + :current_state_ = BT::NodeStatus::SUCCESS; + else (false) + if (ego's position is behind of shift line's start point) then( yes) + :shifting_status_ = BEFORE_SHIFT; + else ( no) + if ( ego's position is between shift line's start point and end point) then (yes) + :shifting_status_ = SHIFTING; + else( no) + :shifting_status_ = AFTER_SHIFT; + endif + endif + :current_state_ = BT::NodeStatus::RUNNING; + endif + stop +} + +@enduml +``` diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 5e2e4673166da..9d19382184adc 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -10,6 +10,7 @@ enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false + enable_safety_check: false # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] @@ -17,17 +18,27 @@ object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] object_check_min_road_shoulder_width: 0.5 # [m] + # For safety check + safety_check_backward_distance: 50.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + # For lateral margin object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] + # For longitudinal margin + longitudinal_collision_margin_buffer: 0.0 # [m] + prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_avoidance_distance: 10.0 # [m] @@ -74,3 +85,9 @@ # ---------- advanced parameters ---------- avoidance_execution_lateral_threshold: 0.499 + + target_velocity_matrix: + col_size: 4 + matrix: + [2.78, 5.56, 11.1, 13.9, # velocity [m/s] + 0.20, 0.90, 1.10, 1.20] # margin [m] diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 68cc14a8299a1..df742e7fd4142 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -2,19 +2,10 @@ ros__parameters: backward_path_length: 5.0 forward_path_length: 200.0 - backward_length_buffer_for_end_of_lane: 5.0 backward_length_buffer_for_end_of_pull_over: 5.0 backward_length_buffer_for_end_of_pull_out: 5.0 - minimum_lane_change_length: 12.0 - minimum_lane_change_prepare_distance: 2.0 # [m] minimum_pull_over_length: 16.0 - drivable_area_resolution: 0.1 - drivable_lane_forward_length: 50.0 - drivable_lane_backward_length: 5.0 - drivable_lane_margin: 3.0 - drivable_area_margin: 6.0 - refine_goal_search_radius_range: 7.5 # parameters for turn signal decider @@ -27,7 +18,7 @@ path_interval: 2.0 - visualize_drivable_area_for_shared_linestrings_lanelet: true + visualize_maximum_drivable_area: true lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index da89dddc6bd8f..8b8c989baec12 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -2,19 +2,33 @@ ros__parameters: lane_change: lane_change_prepare_duration: 4.0 # [s] - lane_changing_duration: 8.0 # [s] - minimum_lane_change_prepare_distance: 4.0 # [m] + lane_changing_safety_check_duration: 8.0 # [s] + + minimum_lane_change_prepare_distance: 2.0 # [m] + minimum_lane_change_length: 16.5 # [m] + backward_length_buffer_for_end_of_lane: 2.0 # [m] lane_change_finish_judge_buffer: 3.0 # [m] - minimum_lane_change_velocity: 5.6 # [m/s] + + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.5 # [m/s2] + + minimum_lane_change_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 - abort_lane_change_velocity_thresh: 0.5 - abort_lane_change_angle_thresh: 10.0 # [deg] - abort_lane_change_distance_thresh: 0.3 # [m] + + # collision check + enable_collision_check_at_prepare_phase: true prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] - enable_abort_lane_change: true - enable_collision_check_at_prepare_phase: false use_predicted_path_outside_lanelet: true - use_all_predicted_path: true + use_all_predicted_path: false + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 1000.0 # [m/s3] + + # debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index 7a9a63f805d67..b092eb7fc8802 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -5,12 +5,11 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margin: 1.0 - pull_out_finish_judge_buffer: 1.0 + collision_check_distance_from_end: 1.0 # shift pull out enable_shift_pull_out: true shift_pull_out_velocity: 2.0 pull_out_sampling_num: 4 - before_pull_out_straight_distance: 0.0 minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 8e277959bc069..75b5facaaaafe 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -35,7 +35,6 @@ minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 after_pull_over_straight_distance: 5.0 - before_pull_over_straight_distance: 5.0 # parallel parking path enable_arc_forward_parking: true enable_arc_backward_parking: true diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg new file mode 100644 index 0000000000000..fd1729013c1ba --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg @@ -0,0 +1,241 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+ + Lane 3 + + + + + + +
+
+
+ Lane 7 +
+
+
+ + Lane 7 + + + + + + +
+
+
+ Lane 2 +
+
+
+ + Lane 2 + + + + + + +
+
+
+ Lane 6 +
+
+
+ + Lane 6 + + + + + + + +
+
+
+ Lane 4 +
+
+
+ + Lane 4 + + + + + + +
+
+
+ Lane 8 +
+
+
+ + Lane 8 + + + + + + +
+
+
+ Lane 10 +
+
+
+ + Lane 10 + + + + + + +
+
+
+ Lane 9 +
+
+
+ + Lane 9 + + + + + + +
+
+
+ Lane 11 +
+
+
+ + Lane 11 + + + + + + +
+
+
+ + Lane 5 + +
+
+
+ + Lane 5 + + + + + + + + + + +
+
+
+ Lane 1 +
+
+
+ + Lane 1 + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg new file mode 100644 index 0000000000000..c1bd0ccdd0e09 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg @@ -0,0 +1,115 @@ + + + + + + + + + + +
+
+
+ + Lane1 +
+ +
+
+
+ + Lane1 + + + + + + + + + +
+
+
+ expanded lanes +
+
+
+ + expanded lanes + + + + + + + + + + +
+
+
+ + Lane2 +
+ +
+
+
+ + Lane2 + + + + + + +
+
+
+ + Lane3 +
+ +
+
+
+ + Lane3 + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg new file mode 100644 index 0000000000000..3c6bc512ff328 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg @@ -0,0 +1,409 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+ + Lane 3 + + + + + + +
+
+
+ Lane 7 +
+
+
+ + Lane 7 + + + + + + +
+
+
+ Lane 2 +
+
+
+ + Lane 2 + + + + + + +
+
+
+ Lane 6 +
+
+
+ + Lane 6 + + + + + + + + + +
+
+
+ Lane 4 +
+
+
+ + Lane 4 + + + + + + +
+
+
+ Lane 8 +
+
+
+ + Lane 8 + + + + + + +
+
+
+ Lane 10 +
+
+
+ + Lane 10 + + + + + + +
+
+
+ Lane 9 +
+
+
+ + Lane 9 + + + + + + +
+
+
+ Lane 11 +
+
+
+ + Lane 11 + + + + + + +
+
+
+ + Lane 5 + +
+
+
+ + Lane 5 + + + + + + + + + + +
+
+
+ Lane 1 +
+
+
+ + Lane 1 + + + + + + +
+
+
+
+ DrivableLanes1 +
+
+ { +
+
+ right_lane = 1 +
+
+ left_lane = 1 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes1... + + + + + + +
+
+
+
+ DrivableLanes2 +
+
+ { +
+
+ right_lane = 4 +
+
+ left_lane = 2 +
+
+ middle_lanes = {3} +
+
+ } +
+
+
+
+ + DrivableLanes2... + + + + + + + + + + +
+
+
+
+ DrivableLanes3 +
+
+ { +
+
+ right_lane = 8 +
+
+ left_lane = 5 +
+
+ middle_lanes = {6,7} +
+
+ } +
+
+
+
+ + DrivableLanes3... + + + + + + + + +
+
+
+
+ DrivableLanes4 +
+
+ { +
+
+ right_lane = 10 +
+
+ left_lane = 9 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes4... + + + + + + +
+
+
+
+ DrivableLanes5 +
+
+ { +
+
+ right_lane = 11 +
+
+ left_lane = 11 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes5... + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png b/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png new file mode 100644 index 0000000000000..0913912b6a332 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png differ diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png b/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png new file mode 100644 index 0000000000000..10b7beb6ad7d0 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png differ diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png new file mode 100644 index 0000000000000..c3780540f8d00 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png differ diff --git a/planning/behavior_path_planner/image/path_shifter.png b/planning/behavior_path_planner/image/path_shifter.png index 91542fdd0d042..d997f2d5ec577 100644 Binary files a/planning/behavior_path_planner/image/path_shifter.png and b/planning/behavior_path_planner/image/path_shifter.png differ diff --git a/planning/behavior_path_planner/image/path_shifter_old.png b/planning/behavior_path_planner/image/path_shifter_old.png new file mode 100644 index 0000000000000..91542fdd0d042 Binary files /dev/null and b/planning/behavior_path_planner/image/path_shifter_old.png differ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 48ac09b77e803..8bf3ef1597d6a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -98,18 +99,24 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; - rclcpp::Publisher::SharedPtr path_candidate_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::TimerBase::SharedPtr timer_; + std::map::SharedPtr> path_candidate_publishers_; + std::shared_ptr planner_data_; std::shared_ptr bt_manager_; std::unique_ptr steering_factor_interface_ptr_; tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; Scenario::SharedPtr current_scenario_{nullptr}; + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; + bool has_received_map_{false}; + bool has_received_route_{false}; + TurnSignalDecider turn_signal_decider_; std::mutex mutex_pd_; // mutex for planner_data_ @@ -118,6 +125,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node // setup bool isDataReady(); + // update planner data + std::shared_ptr createLatestPlannerData(); + // parameters std::shared_ptr avoidance_param_ptr; std::shared_ptr lane_change_param_ptr; @@ -160,18 +170,14 @@ class BehaviorPathPlannerNode : public rclcpp::Node PathWithLaneId::SharedPtr getPath( const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); - /** - * @brief extract path candidate from behavior tree output - */ - PathWithLaneId::SharedPtr getPathCandidate( - const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); - /** * @brief skip smooth goal connection */ bool skipSmoothGoalConnection( const std::vector> & statuses) const; + bool keepInputPoints(const std::vector> & statuses) const; + /** * @brief skip smooth goal connection */ @@ -180,15 +186,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & output); // debug - rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; + rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; - /** - * @brief check path if it is unsafe or forced - */ - bool isForcedCandidatePath() const; - /** * @brief publish steering factor from intersection */ @@ -204,6 +205,18 @@ class BehaviorPathPlannerNode : public rclcpp::Node */ void publishSceneModuleDebugMsg(); + /** + * @brief publish path candidate + */ + void publishPathCandidate( + const std::vector> & scene_modules); + + /** + * @brief convert path with lane id to path for publish path candidate + */ + Path convertToPath( + const std::shared_ptr & path_candidate_ptr, const bool is_ready); + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp index 3d9e2e43ff423..75d80206c4dde 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp @@ -51,6 +51,10 @@ class BehaviorTreeManager BehaviorModuleOutput run(const std::shared_ptr & data); std::vector> getModulesStatus(); std::shared_ptr getAllSceneModuleDebugMsgData(); + std::vector> getSceneModules() const + { + return scene_modules_; + } AvoidanceDebugMsgArray getAvoidanceDebugMsgArray(); @@ -68,6 +72,7 @@ class BehaviorTreeManager BT::Blackboard::Ptr blackboard_; BT::NodeStatus checkForceApproval(const std::string & name); + void resetNotRunningModulePathCandidate(); // For Groot monitoring std::unique_ptr groot_monitor_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 132e01f5d91d3..7caa25a9b4cf6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -31,11 +31,6 @@ struct BehaviorPathPlannerParameters double minimum_pull_out_length; double drivable_area_resolution; - double drivable_lane_forward_length; - double drivable_lane_backward_length; - double drivable_lane_margin; - double drivable_area_margin; - double refine_goal_search_radius_range; double turn_signal_intersection_search_distance; @@ -63,8 +58,8 @@ struct BehaviorPathPlannerParameters double base_link2front; double base_link2rear; - // drivable area visualization - bool visualize_drivable_area_for_shared_linestrings_lanelet; + // maximum drivable area visualization + bool visualize_maximum_drivable_area; // collision check double lateral_distance_max_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index 5ab15f17306aa..2d6f44633cc5d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -41,7 +41,8 @@ std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, const size_t end = std::numeric_limits::max(), const double offset = 0.0); -PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval); +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points = false); Path toPath(const PathWithLaneId & input); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index a4783a445b177..8e812ae915d63 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -35,7 +36,12 @@ namespace behavior_path_planner { + +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; + using tier4_planning_msgs::msg::AvoidanceDebugMsg; + class AvoidanceModule : public SceneModuleInterface { public: @@ -134,10 +140,10 @@ class AvoidanceModule : public SceneModuleInterface const Point ego_position = planner_data_->self_pose->pose.position; for (const auto & left_shift : left_shift_array_) { - const double start_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, left_shift.finish_pose.position); + const double start_distance = + calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); + const double finish_distance = + calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); rtc_interface_left_.updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -148,10 +154,10 @@ class AvoidanceModule : public SceneModuleInterface } for (const auto & right_shift : right_shift_array_) { - const double start_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, right_shift.finish_pose.position); + const double start_distance = + calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); + const double finish_distance = + calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); rtc_interface_right_.updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -200,6 +206,7 @@ class AvoidanceModule : public SceneModuleInterface void fillObjectMovingTime(ObjectData & object_data) const; void compensateDetectionLost( ObjectDataArray & target_objects, ObjectDataArray & other_objects) const; + void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; // data used in previous planning ShiftedPath prev_output_; @@ -225,14 +232,15 @@ class AvoidanceModule : public SceneModuleInterface void updateRegisteredObject(const ObjectDataArray & objects); // -- for shift point generation -- - AvoidLineArray calcShiftLines(AvoidLineArray & current_raw_shift_lines, DebugData & debug) const; + AvoidLineArray applyPreProcessToRawShiftLines( + AvoidLineArray & current_raw_shift_points, DebugData & debug) const; // shift point generation: generator double getShiftLength( const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const; - AvoidLineArray calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const; - double getRightShiftBound() const; - double getLeftShiftBound() const; + + AvoidLineArray calcRawShiftLinesFromObjects( + const AvoidancePlanningData & data, DebugData & debug) const; // shift point generation: combiner AvoidLineArray combineRawShiftLinesWithUniqueCheck( @@ -268,7 +276,7 @@ class AvoidanceModule : public SceneModuleInterface void fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const; // -- for new shift point approval -- - boost::optional findNewShiftLine( + AvoidLineArray findNewShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const; void addShiftLineIfApproved(const AvoidLineArray & point); void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const; @@ -300,33 +308,115 @@ class AvoidanceModule : public SceneModuleInterface void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; - // ===================================== + + double getLateralMarginFromVelocity(const double velocity) const; + + double getRSSLongitudinalDistance( + const double v_ego, const double v_obj, const bool is_front_object) const; + + ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; + + // ========= safety check ============== + + lanelet::ConstLanelets getAdjacentLane( + const PathShifter & path_shifter, const double forward_distance, + const double backward_distance) const; + + bool isSafePath( + const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const; + + bool isSafePath( + const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, + DebugData & debug) const; + + bool isEnoughMargin( + const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, + MarginData & margin_data) const; + // ========= helper functions ========== - // ===================================== + + double getEgoSpeed() const + { + return std::abs(planner_data_->self_odometry->twist.twist.linear.x); + } + + double getNominalAvoidanceEgoSpeed() const + { + return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); + } + + double getSharpAvoidanceEgoSpeed() const + { + return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); + } + + double getNominalPrepareDistance() const + { + const auto & p = parameters_; + const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. + const auto nominal_distance = + std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); + return nominal_distance + epsilon_m; + } + + double getNominalAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getSharpAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getRightShiftBound() const + { + // TODO(Horibe) write me. Real lane boundary must be considered here. + return -parameters_->max_right_shift_length; + } + + double getLeftShiftBound() const + { + // TODO(Horibe) write me. Real lane boundary must be considered here. + return parameters_->max_left_shift_length; + } + + double getCurrentShift() const + { + return prev_output_.shift_length.at( + findNearestIndex(prev_output_.path.points, getEgoPosition())); + } + + double getCurrentLinearShift() const + { + return prev_linear_shift_path_.shift_length.at( + findNearestIndex(prev_linear_shift_path_.path.points, getEgoPosition())); + } + + double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } + + Point getEgoPosition() const { return planner_data_->self_pose->pose.position; } + + Pose getEgoPose() const { return planner_data_->self_pose->pose; } + + Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; PathWithLaneId calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const; + const std::shared_ptr & planner_data, const Pose & pose) const; // TODO(Horibe): think later. // for unique ID mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; } - double getNominalAvoidanceDistance(const double shift_length) const; - double getNominalPrepareDistance() const; - double getNominalAvoidanceEgoSpeed() const; - - double getSharpAvoidanceDistance(const double shift_length) const; - double getSharpAvoidanceEgoSpeed() const; - - double getEgoSpeed() const; - Point getEgoPosition() const; - PoseStamped getEgoPose() const; - PoseStamped getUnshiftedEgoPose(const ShiftedPath & prev_path) const; - double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - double getCurrentShift() const; - double getCurrentLinearShift() const; - /** * avoidance module misc data */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 25b29352973c1..fb3560a6bfe90 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -26,6 +26,7 @@ #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Point2d; @@ -68,6 +70,9 @@ struct AvoidanceParameters // enable update path when if detected objects on planner data is gone. bool enable_update_path_when_object_is_gone{false}; + // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver + bool enable_safety_check{false}; + // Vehicles whose distance to the center of the path is // less than this will not be considered for avoidance. double threshold_distance_object_is_on_center; @@ -81,6 +86,10 @@ struct AvoidanceParameters // continue to detect backward vehicles as avoidance targets until they are this distance away double object_check_backward_distance; + // if the distance between object and goal position is less than this parameter, the module ignore + // the object. + double object_check_goal_distance; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio; @@ -99,6 +108,9 @@ struct AvoidanceParameters // don't ever set this value to 0 double lateral_collision_safety_buffer{0.5}; + // margin between object back and end point of avoid shift point + double longitudinal_collision_safety_buffer{0.0}; + // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction double longitudinal_collision_margin_min_distance; @@ -107,6 +119,21 @@ struct AvoidanceParameters // for longitudinal direction double longitudinal_collision_margin_time; + // find adjacent lane vehicles + double safety_check_backward_distance; + + // minimum longitudinal margin for vehicles in adjacent lane + double safety_check_min_longitudinal_margin; + + // safety check time horizon + double safety_check_time_horizon; + + // use in RSS calculation + double safety_check_idling_time; + + // use in RSS calculation + double safety_check_accel_for_rss; + // start avoidance after this time to avoid sudden path change double prepare_time; @@ -163,6 +190,12 @@ struct AvoidanceParameters // avoidance path will be generated unless their lateral margin difference exceeds this value. double avoidance_execution_lateral_threshold; + // target velocity matrix + std::vector target_velocity_matrix; + + // matrix col size + size_t col_size; + // true by default bool avoid_car{true}; // avoidance is performed for type object car bool avoid_truck{true}; // avoidance is performed for type object truck @@ -234,6 +267,9 @@ struct ObjectData // avoidance target // lateral distance from overhang to the road shoulder double to_road_shoulder_distance{0.0}; + + // unavoidable reason + std::string reason{""}; }; using ObjectDataArray = std::vector; @@ -286,11 +322,24 @@ struct AvoidancePlanningData // current driving lanelet lanelet::ConstLanelets current_lanelets; + // output path + ShiftedPath candidate_path; + // avoidance target objects ObjectDataArray target_objects; // the others ObjectDataArray other_objects; + + // raw shift point + AvoidLineArray unapproved_raw_sl{}; + + // new shift point + AvoidLineArray unapproved_new_sl{}; + + bool safe{false}; + + bool avoiding_now{false}; }; /* @@ -317,6 +366,27 @@ struct ShiftLineData std::vector> shift_line_history; }; +/* + * Data struct for longitudinal margin + */ +struct MarginData +{ + Pose pose{}; + + bool enough_lateral_margin{true}; + + double longitudinal_distance{std::numeric_limits::max()}; + + double longitudinal_margin{std::numeric_limits::lowest()}; + + double vehicle_width; + + double base_link2front; + + double base_link2rear; +}; +using MarginDataArray = std::vector; + /* * Debug information for marker array */ @@ -345,6 +415,20 @@ struct DebugData std::vector total_shift; std::vector output_shift; + bool exist_adjacent_objects{false}; + + // future pose + PathWithLaneId path_with_planned_velocity; + + // margin + MarginDataArray margin_data_array; + + // avoidance require objects + ObjectDataArray unavoidable_objects; + + // avoidance unsafe objects + ObjectDataArray unsafe_objects; + // tmp for plot PathWithLaneId center_line; AvoidanceDebugMsgArray avoidance_debug_msg_array; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index ce04323a775dc..5e2ecf2e61211 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -101,6 +101,15 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const ObjectDataArray & objects, const bool enable_bound_clipping); + +double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); + +lanelet::ConstLanelets getTargetLanelets( + const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, + const double left_offset, const double right_offset); } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index 60e31b0a3a416..5d1ff1254eb24 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -36,6 +36,7 @@ namespace marker_utils::avoidance_marker { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::ShiftLineArray; @@ -44,15 +45,22 @@ using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; +MarkerArray createEgoStatusMarkerArray( + const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); + MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, const float & b, const double & w); -MarkerArray createTargetObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); +MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); -MarkerArray createOtherObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); +MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); + +MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); + +MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); + +MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 39de619c4e3eb..0ce55c2796f08 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -54,8 +54,6 @@ class LaneChangeModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); - BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; bool isExecutionReady() const override; BT::NodeStatus updateState() override; @@ -103,16 +101,24 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; + LaneChangeStates current_lane_change_state_; + std::shared_ptr abort_path_; + PathWithLaneId prev_approved_path_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; + bool is_abort_path_approved_{false}; + bool is_abort_approval_requested_{false}; + bool is_abort_condition_satisfied_{false}; + bool is_activated_ = false; RTCInterface rtc_interface_left_; RTCInterface rtc_interface_right_; UUID uuid_left_; UUID uuid_right_; + UUID candidate_uuid_; - bool is_activated_ = false; + void resetParameters(); void waitApprovalLeft(const double start_distance, const double finish_distance) { @@ -134,12 +140,14 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_left_.updateCooperateStatus( uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_left_; return; } if (candidate.lateral_shift < 0.0) { rtc_interface_right_.updateCooperateStatus( uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_right_; return; } @@ -154,6 +162,21 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } + void removePreviousRTCStatusLeft() + { + if (rtc_interface_left_.isRegistered(uuid_left_)) { + rtc_interface_left_.removeCooperateStatus(uuid_left_); + } + } + + void removePreviousRTCStatusRight() + { + if (rtc_interface_right_.isRegistered(uuid_right_)) { + rtc_interface_right_.removeCooperateStatus(uuid_right_); + } + } + + lanelet::ConstLanelets get_original_lanes() const; PathWithLaneId getReferencePath() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; @@ -165,6 +188,7 @@ class LaneChangeModule : public SceneModuleInterface void generateExtendedDrivableArea(PathWithLaneId & path); void updateOutputTurnSignal(BehaviorModuleOutput & output); void updateSteeringFactorPtr(const BehaviorModuleOutput & output); + bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; @@ -172,20 +196,20 @@ class LaneChangeModule : public SceneModuleInterface bool isValidPath(const PathWithLaneId & path) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; - bool isAbortConditionSatisfied() const; + bool isAbortConditionSatisfied(); bool hasFinishedLaneChange() const; - void resetParameters(); + bool isAbortState() const; // getter Pose getEgoPose() const; Twist getEgoTwist() const; std_msgs::msg::Header getRouteHeader() const; + void resetPathIfAbort(); // debug + void setObjectDebugVisualization() const; mutable std::unordered_map object_debug_; mutable std::vector debug_valid_path_; - - void setObjectDebugVisualization() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index be14bb120aa42..dfcb0f5e24c3b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -25,25 +25,36 @@ namespace behavior_path_planner { struct LaneChangeParameters { + // trajectory generation double lane_change_prepare_duration{2.0}; - double lane_changing_duration{4.0}; + double lane_changing_safety_check_duration{4.0}; + double lane_changing_lateral_jerk{0.5}; + double lane_changing_lateral_acc{0.5}; double lane_change_finish_judge_buffer{3.0}; double minimum_lane_change_velocity{5.6}; double prediction_time_resolution{0.5}; double maximum_deceleration{1.0}; int lane_change_sampling_num{10}; - double abort_lane_change_velocity_thresh{0.5}; - double abort_lane_change_angle_thresh{0.174533}; - double abort_lane_change_distance_thresh{0.3}; - double prepare_phase_ignore_target_speed_thresh{0.1}; - bool enable_abort_lane_change{true}; + + // collision check bool enable_collision_check_at_prepare_phase{true}; + double prepare_phase_ignore_target_speed_thresh{0.1}; bool use_predicted_path_outside_lanelet{false}; bool use_all_predicted_path{false}; - bool publish_debug_marker{false}; + + // abort + bool enable_cancel_lane_change{true}; + bool enable_abort_lane_change{false}; + + double abort_delta_time{3.0}; + double abort_max_lateral_jerk{10.0}; + // drivable area expansion double drivable_area_right_bound_offset{0.0}; double drivable_area_left_bound_offset{0.0}; + + // debug marker + bool publish_debug_marker{false}; }; struct LaneChangeStatus @@ -57,6 +68,14 @@ struct LaneChangeStatus bool is_safe; double start_distance; }; + +enum class LaneChangeStates { + Normal = 0, + Cancel, + Abort, + Stop, +}; + } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index e346abbf6beeb..83de5e24d5dba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp @@ -29,12 +29,15 @@ using behavior_path_planner::TurnSignalInfo; struct LaneChangePath { PathWithLaneId path; + lanelet::ConstLanelets reference_lanelets; + lanelet::ConstLanelets target_lanelets; ShiftedPath shifted_path; ShiftLine shift_line; double acceleration{0.0}; double preparation_length{0.0}; double lane_change_length{0.0}; TurnSignalInfo turn_signal_info; + PathWithLaneId prev_path; }; using LaneChangePaths = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 1e15ee93a7d19..4c8a7e9fb91a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -15,7 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ +#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utilities.hpp" @@ -31,6 +33,7 @@ #include #include #include +#include #include namespace behavior_path_planner::lane_change_utils @@ -53,18 +56,18 @@ double getExpectedVelocityWhenDecelerate( const double & current_velocity, const double & expected_acceleration, const double & lane_change_prepare_duration); -double getDistanceWhenDecelerate( - const double & velocity, const double & expected_acceleration, const double & duration, - const double & minimum_distance); +std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( + const double velocity, const double shift_length, const double deceleration, + const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, + const LaneChangeParameters & lc_param); std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double & acceleration, const double & prepare_distance, const double & prepare_duration, - const double & prepare_speed, const double & minimum_prepare_distance, - const double & lane_change_distance, const double & lane_changing_duration, - const double & minimum_lane_change_velocity); + const double acceleration, const double prepare_distance, const double prepare_duration, + const double prepare_speed, const double lane_change_distance, const double lane_changing_speed, + const BehaviorPathPlannerParameters & params, const LaneChangeParameters & lane_change_param); LaneChangePaths getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, @@ -94,7 +97,7 @@ bool isLaneChangePathSafe( const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & lane_change_parameters, - const double front_decel, const double rear_decel, + const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, std::unordered_map & debug_data, const bool use_buffer = true, const double acceleration = 0.0); @@ -125,19 +128,11 @@ PathWithLaneId getLaneChangePathPrepareSegment( PathWithLaneId getLaneChangePathLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const Pose & current_pose, const double & forward_path_length, const double & prepare_distance, - const double & lane_change_distance, const double & minimum_lane_change_length, - const double & lane_change_distance_buffer, const double & lane_changing_duration, - const double & minimum_lane_change_velocity); + const Pose & current_pose, const double prepare_distance, const double lane_change_distance, + const double lane_changing_speed, const BehaviorPathPlannerParameters & common_param); bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param); -bool isEgoDistanceNearToCenterline( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param); -bool isEgoHeadingAngleLessThanThreshold( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param); TurnSignalInfo calc_turn_signal_info( const PathWithLaneId & prepare_path, const double prepare_velocity, @@ -150,6 +145,18 @@ void get_turn_signal_info( std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); + +std::optional getAbortPaths( + const std::shared_ptr & planner_data, const LaneChangePath & selected_path, + const Pose & ego_lerp_pose_before_collision, const BehaviorPathPlannerParameters & common_param, + const LaneChangeParameters & lane_change_param); + +double getLateralShift(const LaneChangePath & path); + +bool hasEnoughDistanceToLaneChangeAfterAbort( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const Pose & curent_pose, const double abort_return_dist, + const BehaviorPathPlannerParameters & common_param); } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 92cd395e159dd..f7e3413f8f40c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -71,6 +71,10 @@ class PullOutModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; BT::NodeStatus updateState() override; + BT::NodeStatus getNodeStatusWhileWaitingApproval() const override + { + return BT::NodeStatus::SUCCESS; + } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -114,6 +118,7 @@ class PullOutModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose); void planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose); + void generateStopPath(); void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index 96f76278e50df..21b66d1ad8457 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -26,12 +26,11 @@ struct PullOutParameters double th_stopped_velocity; double th_stopped_time; double collision_check_margin; - double pull_out_finish_judge_buffer; + double collision_check_distance_from_end; // shift pull out bool enable_shift_pull_out; double shift_pull_out_velocity; int pull_out_sampling_num; - double before_pull_out_straight_distance; double minimum_shift_pull_out_distance; double maximum_lateral_jerk; double minimum_lateral_jerk; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp index 4f43bb080fabd..7181f9bd8e9bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp @@ -49,6 +49,9 @@ class ShiftPullOut : public PullOutPlannerBase const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); + double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr); + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp index 5ab414f0cb90e..b0a909a36ac13 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp @@ -18,12 +18,16 @@ #include "behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp" #include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" + #include #include namespace behavior_path_planner { +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; +using BasicPolygons2d = std::vector; class GoalSearcher : public GoalSearcherBase { @@ -32,12 +36,17 @@ class GoalSearcher : public GoalSearcherBase const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); - std::vector search(const Pose & original_goal_pose) override; + GoalCandidates search(const Pose & original_goal_pose) override; + void update(GoalCandidates & goal_candidates) const override; + +private: + void createAreaPolygons(std::vector original_search_poses); bool checkCollision(const Pose & pose) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; + bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; -private: LinearRing2d vehicle_footprint_{}; std::shared_ptr occupancy_grid_map_{}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp index 261e586c7c123..3bea5528c7123 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp @@ -28,22 +28,28 @@ namespace behavior_path_planner { using geometry_msgs::msg::Pose; +using tier4_autoware_utils::MultiPolygon2d; struct GoalCandidate { Pose goal_pose{}; double distance_from_original_goal{0.0}; double lateral_offset{0.0}; + size_t id{0}; + bool is_safe{true}; bool operator<(const GoalCandidate & other) const noexcept { - // compare in order of decreasing lateral offset. - if (std::abs(lateral_offset - other.lateral_offset) > std::numeric_limits::epsilon()) { + const double diff = distance_from_original_goal - other.distance_from_original_goal; + constexpr double eps = 0.01; + if (std::abs(diff) < eps) { return lateral_offset < other.lateral_offset; } + return distance_from_original_goal < other.distance_from_original_goal; } }; +using GoalCandidates = std::vector; class GoalSearcherBase { @@ -56,11 +62,14 @@ class GoalSearcherBase planner_data_ = planner_data; } - virtual std::vector search(const Pose & original_goal_pose) = 0; + MultiPolygon2d getAreaPolygons() { return area_polygons_; } + virtual GoalCandidates search(const Pose & original_goal_pose) = 0; + virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: PullOverParameters parameters_; std::shared_ptr planner_data_; + MultiPolygon2d area_polygons_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 7047c102b5bf2..f86f592f0560f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -53,7 +53,6 @@ enum PathType { struct PUllOverStatus { - std::shared_ptr planner{}; PullOverPath pull_over_path{}; size_t current_path_idx{0}; std::shared_ptr prev_stop_path{nullptr}; @@ -65,6 +64,7 @@ struct PUllOverStatus bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; + std::optional stop_pose{}; }; class PullOverModule : public SceneModuleInterface @@ -78,9 +78,10 @@ class PullOverModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; BT::NodeStatus updateState() override; - void onTimer(); - bool planWithEfficientPath(); - bool planWithCloseGoal(); + BT::NodeStatus getNodeStatusWhileWaitingApproval() const override + { + return BT::NodeStatus::SUCCESS; + } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -103,9 +104,6 @@ class PullOverModule : public SceneModuleInterface PullOverPath shift_parking_path_; vehicle_info_util::VehicleInfo vehicle_info_; - const double pull_over_lane_length_{200.0}; - const double check_distance_{100.0}; - rclcpp::Subscription::SharedPtr occupancy_grid_sub_; rclcpp::Publisher::SharedPtr goal_pose_pub_; @@ -113,7 +111,9 @@ class PullOverModule : public SceneModuleInterface std::shared_ptr occupancy_grid_map_; Pose modified_goal_pose_; Pose refined_goal_pose_; - std::vector goal_candidates_; + GoalCandidates goal_candidates_; + std::vector pull_over_path_candidates_; + std::optional closest_start_pose_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; std::deque odometry_buffer_; @@ -124,17 +124,11 @@ class PullOverModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; - PathWithLaneId getFullPath() const; - PathWithLaneId getReferencePath() const; - PathWithLaneId generateStopPath() const; - Pose calcRefinedGoal() const; + Pose calcRefinedGoal(const Pose & goal_pose) const; ParallelParkingParameters getGeometricPullOverParameters() const; - bool isLongEnoughToParkingStart( - const PathWithLaneId & path, const Pose & parking_start_pose) const; - bool isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; - double calcMinimumShiftPathDistance() const; std::pair calcDistanceToPathChange() const; + PathWithLaneId generateStopPath(); + PathWithLaneId generateEmergencyStopPath(); bool isStopped(); bool hasFinishedCurrentPath(); @@ -142,8 +136,17 @@ class PullOverModule : public SceneModuleInterface void updateOccupancyGrid(); void resetStatus(); + bool checkCollision(const PathWithLaneId & path) const; + bool hasEnoughDistance(const PullOverPath & pull_over_path) const; + TurnSignalInfo calcTurnSignalInfo() const; + // timer for generating pull over path candidates + void onTimer(); + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::CallbackGroup::SharedPtr timer_cb_group_; + std::mutex mutex_; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp index 5dc0876877e81..e0c7d5b86f9f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -56,7 +56,6 @@ struct PullOverParameters double pull_over_velocity; double pull_over_minimum_velocity; double after_pull_over_straight_distance; - double before_pull_over_straight_distance; // parallel parking bool enable_arc_forward_parking; bool enable_arc_backward_parking; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp index cfb4e488da1da..92d4d872a4010 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp @@ -43,10 +43,44 @@ enum class PullOverPlannerType { struct PullOverPath { - PathWithLaneId path{}; + PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; Pose start_pose{}; Pose end_pose{}; + std::vector debug_poses{}; + size_t goal_id{}; + + PathWithLaneId getFullPath() const + { + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + path.points = motion_utils::removeOverlapPoints(path.points); + + return path; + } + + PathWithLaneId getParkingPath() const + { + const PathWithLaneId full_path = getFullPath(); + const size_t start_idx = motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId parking_path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(parking_path.points)); + + return parking_path; + } }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp index 19f723bd82385..148e58d3ba904 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp @@ -41,21 +41,18 @@ class ShiftPullOver : public PullOverPlannerBase boost::optional plan(const Pose & goal_pose) override; protected: - PathWithLaneId generateRoadLaneReferencePath( - const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose, - const double pull_over_distance) const; - PathWithLaneId generateShoulderLaneReferencePath( - const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose, - const Pose & goal_pose, const double shoulder_center_to_goal_distance) const; + PathWithLaneId generateReferencePath( + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; boost::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk, - const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance, - const double shoulder_left_bound_to_goal_distance) const; - bool hasEnoughDistance( - const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, - const Pose & goal_pose, const double pull_over_distance) const; - bool isSafePath(const PathWithLaneId & path) const; + const Pose & goal_pose, const double lateral_jerk) const; + static double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); + static std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + static std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index 343938898e7b9..2dbef9948f368 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -50,16 +50,15 @@ PredictedObjects filterObjectsByLateralDistance( const double distance_thresh, const bool filter_inside); // debug -Marker createPullOverAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id, - const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, - const double vehicle_width, const std_msgs::msg::ColorRGBA & color); +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( - std::vector & goal_candidates, const std_msgs::msg::ColorRGBA & color); + GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index d86461911a0c2..65be40b9fb603 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -62,9 +62,6 @@ struct BehaviorModuleOutput // path planed by module PlanResult path{}; - // path candidate planed by module - PlanResult path_candidate{}; - TurnSignalInfo turn_signal_info{}; }; @@ -106,6 +103,15 @@ class SceneModuleInterface */ virtual BT::NodeStatus updateState() = 0; + /** + * @brief If the module plan customized reference path while waiting approval, it should output + * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. + */ + virtual BT::NodeStatus getNodeStatusWhileWaitingApproval() const + { + return BT::NodeStatus::FAILURE; + } + /** * @brief Return true if the module has request for execution (not necessarily feasible) */ @@ -130,7 +136,7 @@ class SceneModuleInterface BehaviorModuleOutput out; out.path = util::generateCenterLinePath(planner_data_); const auto candidate = planCandidate(); - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); return out; } @@ -228,6 +234,10 @@ class SceneModuleInterface bool isWaitingApproval() const { return is_waiting_approval_; } + PlanResult getPathCandidate() const { return path_candidate_; } + + void resetPathCandidate() { path_candidate_.reset(); } + virtual void lockRTCCommand() { if (!rtc_interface_ptr_) { @@ -258,6 +268,7 @@ class SceneModuleInterface std::unique_ptr steering_factor_interface_ptr_; UUID uuid_; bool is_waiting_approval_; + PlanResult path_candidate_; void updateRTCStatus(const double start_distance, const double finish_distance) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index c6703a589e7d6..52073c7f50cbe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -34,6 +34,8 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; +enum class SideShiftStatus { STOP = 0, BEFORE_SHIFT, SHIFTING, AFTER_SHIFT }; + struct SideShiftParameters { double time_to_start_shifting; @@ -87,7 +89,7 @@ class SideShiftModule : public SceneModuleInterface ShiftLine calcShiftLine() const; - bool addShiftLine(); + void replaceShiftLine(); // const methods void publishPath(const PathWithLaneId & path) const; @@ -100,8 +102,17 @@ class SideShiftModule : public SceneModuleInterface lanelet::ConstLanelets current_lanelets_; SideShiftParameters parameters_; - // Current lateral offset to shift the reference path. - double lateral_offset_{0.0}; + // Requested lateral offset to shift the reference path. + double requested_lateral_offset_{0.0}; + + // Inserted lateral offset to shift the reference path. + double inserted_lateral_offset_{0.0}; + + // Inserted shift lines in the path + ShiftLine inserted_shift_line_; + + // Shift status + SideShiftStatus shift_status_; // Flag to check lateral offset change is requested bool lateral_offset_change_request_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index 09786ee180382..3f8e20fa59659 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -101,7 +101,6 @@ class GeometricParallelParking size_t current_path_idx_ = 0; void clearPaths(); - bool isEnoughDistanceToStart(const Pose & start_pose) const; std::vector planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_r, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp index 87ea4430266b2..09922a98d0b0b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp @@ -26,8 +26,8 @@ #include #include +#include #include - namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathPointWithLaneId; @@ -72,6 +72,16 @@ class PathShifter */ void setPath(const PathWithLaneId & path); + /** + * @brief Set velocity used to apply a lateral acceleration limit. + */ + void setVelocity(const double velocity); + + /** + * @brief Set acceleration limit + */ + void setLateralAccelerationLimit(const double acc); + /** * @brief Add shift point. You don't have to care about the start/end_idx. */ @@ -122,65 +132,23 @@ class PathShifter //////////////////////////////////////// static double calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity) - { - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; - } - - static double calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity) - { - constexpr double ep = 1.0e-3; - const double lat = std::abs(lateral); - const double lon = std::max(std::abs(longitudinal), ep); - const double v = std::abs(velocity); - return 0.5 * lat * std::pow(4.0 * v / lon, 3); - } + const double lateral, const double jerk, const double velocity); - double getTotalShiftLength() const - { - double sum = base_offset_; - for (const auto & l : shift_lines_) { - sum += l.end_shift_length; - } - return sum; - } - - double getLastShiftLength() const - { - if (shift_lines_.empty()) { - return base_offset_; - } + static double calcShiftTimeFromJerkAndJerk( + const double lateral, const double jerk, const double acc); - const auto furthest = std::max_element( - shift_lines_.begin(), shift_lines_.end(), - [](auto & a, auto & b) { return a.end_idx < b.end_idx; }); + static double calcJerkFromLatLonDistance( + const double lateral, const double longitudinal, const double velocity); - return furthest->end_shift_length; - } + double getTotalShiftLength() const; - boost::optional getLastShiftLine() const - { - if (shift_lines_.empty()) { - return {}; - } + double getLastShiftLength() const; - const auto furthest = std::max_element( - shift_lines_.begin(), shift_lines_.end(), - [](auto & a, auto & b) { return a.end_idx > b.end_idx; }); - - return *furthest; - } + boost::optional getLastShiftLine() const; /** * @brief Calculate the theoretical lateral jerk by spline shifting for current shift_lines_. - * @return Jerk array. THe size is same as the shift points. + * @return Jerk array. The size is same as the shift points. */ std::vector calcLateralJerk() const; @@ -194,6 +162,12 @@ class PathShifter // The amount of shift length to the entire path. double base_offset_{0.0}; + // Used to apply a lateral acceleration limit + double velocity_{0.0}; + + // lateral acceleration limit considered in the path planning + double acc_limit_{-1.0}; + // Logger mutable rclcpp::Logger logger_{ rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")}; @@ -201,6 +175,12 @@ class PathShifter // Clock mutable rclcpp::Clock clock_{RCL_ROS_TIME}; + std::pair, std::vector> calcBaseLengths( + const double arclength, const double shift_length, const bool offset_back) const; + + std::pair, std::vector> getBaseLengthsWithoutAccelLimit( + const double arclength, const double shift_length, const bool offset_back) const; + /** * @brief Calculate path index for shift_lines and set is_index_aligned_ to true. */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 3697ad5576b1c..ca4f924677b2d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include namespace behavior_path_planner::util @@ -67,6 +68,7 @@ using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; +using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -87,7 +89,7 @@ struct ProjectedDistancePoint double distance{0.0}; }; -template > +template > ProjectedDistancePoint pointToSegment( const Point2d & reference_point, const Point2d & point_from_ego, const Point2d & point_from_object); @@ -197,6 +199,9 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_p bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon); +bool calcObjectPolygon( + const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon); + bool calcObjectPolygon( const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon); @@ -259,13 +264,19 @@ std::vector filterObjectIndicesByLanelets( const double start_arc_length, const double end_arc_length); /** - * @brief Get index of the obstacles inside the lanelets - * @return Indices corresponding to the obstacle inside the lanelets + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. */ -std::vector filterObjectIndicesByLanelets( +std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -PredictedObjects filterObjectsByLanelets( +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); std::vector filterObjectsIndicesByPath( @@ -294,9 +305,9 @@ std::vector cutOverlappedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, - const std::shared_ptr planner_data); + const std::shared_ptr planner_data, const bool is_driving_forward = true); -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); /** @@ -358,6 +369,9 @@ PathPointWithLaneId insertStopPoint(double length, PathWithLaneId * path); double getSignedDistanceFromShoulderLeftBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose); +std::optional getSignedDistanceFromShoulderLeftBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose); double getSignedDistanceFromRightBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose); @@ -467,7 +481,7 @@ bool isSafeInLaneletCollisionCheck( const double check_start_time, const double check_end_time, const double check_time_resolution, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug); + const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug); bool isSafeInFreeSpaceCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, @@ -478,11 +492,12 @@ bool isSafeInFreeSpaceCollisionCheck( bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); -double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param); +double calcTotalLaneChangeDistance( + const BehaviorPathPlannerParameters & common_param, const bool include_buffer = true); double calcLaneChangeBuffer( const BehaviorPathPlannerParameters & common_param, const int num_lane_change, - const double length_to_intersection); + const double length_to_intersection = 0.0); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ 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 73b19ef7ee65f..df89958e37c18 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -61,7 +61,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // publisher path_publisher_ = create_publisher("~/output/path", 1); - path_candidate_publisher_ = create_publisher("~/output/path_candidate", 1); turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); @@ -70,9 +69,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_lane_change_msg_array_publisher_ = create_publisher("~/debug/lane_change_debug_message_array", 1); - if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - debug_drivable_area_lanelets_publisher_ = - create_publisher("~/drivable_area_boundary", 1); + if (planner_data_->parameters.visualize_maximum_drivable_area) { + debug_maximum_drivable_area_publisher_ = + create_publisher("~/maximum_drivable_area", 1); } bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -115,6 +114,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); // behavior tree manager { + const std::string path_candidate_name_space = "/planning/path_candidate/"; mutex_bt_.lock(); bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); @@ -125,6 +125,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto avoidance_module = std::make_shared("Avoidance", *this, avoidance_param_ptr); + path_candidate_publishers_.emplace( + "Avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); bt_manager_->registerSceneModule(avoidance_module); auto lane_following_module = @@ -133,12 +135,18 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto lane_change_module = std::make_shared("LaneChange", *this, lane_change_param_ptr); + path_candidate_publishers_.emplace( + "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); bt_manager_->registerSceneModule(lane_change_module); auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); + path_candidate_publishers_.emplace( + "PullOver", create_publisher(path_candidate_name_space + "pull_over", 1)); bt_manager_->registerSceneModule(pull_over_module); auto pull_out_module = std::make_shared("PullOut", *this, getPullOutParam()); + path_candidate_publishers_.emplace( + "PullOut", create_publisher(path_candidate_name_space + "pull_out", 1)); bt_manager_->registerSceneModule(pull_out_module); bt_manager_->createBehaviorTree(); @@ -201,21 +209,16 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.backward_path_length = declare_parameter("backward_path_length", 5.0) + backward_offset; p.forward_path_length = declare_parameter("forward_path_length", 100.0); p.backward_length_buffer_for_end_of_lane = - declare_parameter("backward_length_buffer_for_end_of_lane", 5.0); + declare_parameter("lane_change.backward_length_buffer_for_end_of_lane", 5.0); p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over", 5.0); p.backward_length_buffer_for_end_of_pull_out = declare_parameter("backward_length_buffer_for_end_of_pull_out", 5.0); - p.minimum_lane_change_length = declare_parameter("minimum_lane_change_length", 8.0); + p.minimum_lane_change_length = declare_parameter("lane_change.minimum_lane_change_length", 8.0); p.minimum_lane_change_prepare_distance = - declare_parameter("minimum_lane_change_prepare_distance", 2.0); + declare_parameter("lane_change.minimum_lane_change_prepare_distance", 2.0); p.minimum_pull_over_length = declare_parameter("minimum_pull_over_length", 15.0); - p.drivable_area_resolution = declare_parameter("drivable_area_resolution"); - p.drivable_lane_forward_length = declare_parameter("drivable_lane_forward_length"); - p.drivable_lane_backward_length = declare_parameter("drivable_lane_backward_length"); - p.drivable_lane_margin = declare_parameter("drivable_lane_margin"); - p.drivable_area_margin = declare_parameter("drivable_area_margin"); p.refine_goal_search_radius_range = declare_parameter("refine_goal_search_radius_range", 7.5); p.turn_signal_intersection_search_distance = declare_parameter("turn_signal_intersection_search_distance", 30.0); @@ -229,8 +232,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving", true); p.path_interval = declare_parameter("path_interval"); - p.visualize_drivable_area_for_shared_linestrings_lanelet = - declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); + p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area", true); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -286,17 +288,26 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true); p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true); p.enable_update_path_when_object_is_gone = dp("enable_update_path_when_object_is_gone", false); + p.enable_safety_check = dp("enable_safety_check", false); p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0); p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0); p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0); p.object_check_forward_distance = dp("object_check_forward_distance", 150.0); p.object_check_backward_distance = dp("object_check_backward_distance", 2.0); + p.object_check_goal_distance = dp("object_check_goal_distance", 20.0); p.object_check_shiftable_ratio = dp("object_check_shiftable_ratio", 1.0); p.object_check_min_road_shoulder_width = dp("object_check_min_road_shoulder_width", 0.5); p.object_envelope_buffer = dp("object_envelope_buffer", 0.1); p.lateral_collision_margin = dp("lateral_collision_margin", 2.0); p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5); + p.longitudinal_collision_safety_buffer = dp("longitudinal_collision_safety_buffer", 0.0); + + p.safety_check_min_longitudinal_margin = dp("safety_check_min_longitudinal_margin", 0.0); + p.safety_check_backward_distance = dp("safety_check_backward_distance", 0.0); + p.safety_check_time_horizon = dp("safety_check_time_horizon", 10.0); + p.safety_check_idling_time = dp("safety_check_idling_time", 1.5); + p.safety_check_accel_for_rss = dp("safety_check_accel_for_rss", 2.5); p.prepare_time = dp("prepare_time", 3.0); p.min_prepare_distance = dp("min_prepare_distance", 10.0); @@ -325,6 +336,13 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.publish_debug_marker = dp("publish_debug_marker", false); p.print_debug_info = dp("print_debug_info", false); + // velocity matrix + { + std::string ns = "avoidance.target_velocity_matrix."; + p.col_size = declare_parameter(ns + "col_size"); + p.target_velocity_matrix = declare_parameter>(ns + "matrix"); + } + p.avoid_car = dp("target_object.car", true); p.avoid_truck = dp("target_object.truck", true); p.avoid_bus = dp("target_object.bus", true); @@ -364,26 +382,38 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() }; LaneChangeParameters p{}; + + // trajectory generation p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); - p.lane_changing_duration = dp("lane_changing_duration", 4.0); + p.lane_changing_safety_check_duration = dp("lane_changing_safety_check_duration", 4.0); + p.lane_changing_lateral_jerk = dp("lane_changing_lateral_jerk", 0.5); + p.lane_changing_lateral_acc = dp("lane_changing_lateral_acc", 0.5); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 5.6); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); - p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5); - p.abort_lane_change_angle_thresh = - dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); - p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); - p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1); - p.enable_abort_lane_change = dp("enable_abort_lane_change", true); + + // collision check p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); + p.prepare_phase_ignore_target_speed_thresh = dp("prepare_phase_ignore_target_speed_thresh", 0.1); p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); p.use_all_predicted_path = dp("use_all_predicted_path", true); - p.publish_debug_marker = dp("publish_debug_marker", false); + + // abort + p.enable_cancel_lane_change = dp("enable_cancel_lane_change", true); + p.enable_abort_lane_change = dp("enable_abort_lane_change", false); + + p.abort_delta_time = dp("abort_delta_time", 3.0); + p.abort_max_lateral_jerk = dp("abort_max_lateral_jerk", 10.0); + + // drivable area expansion p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); + // debug marker + p.publish_debug_marker = dp("publish_debug_marker", false); + // validation of parameters if (p.lane_change_sampling_num < 1) { RCLCPP_FATAL_STREAM( @@ -400,6 +430,13 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() exit(EXIT_FAILURE); } + if (p.abort_delta_time < 1.0) { + RCLCPP_FATAL_STREAM( + get_logger(), "abort_delta_time: " << p.abort_delta_time << ", is too short.\n" + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + return p; } @@ -447,7 +484,6 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.pull_over_velocity = dp("pull_over_velocity", 8.3); p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3); p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0); - p.before_pull_over_straight_distance = dp("before_pull_over_straight_distance", 3.0); // parallel parking p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); @@ -510,12 +546,11 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); p.th_stopped_time = dp("th_stopped_time", 1.0); p.collision_check_margin = dp("collision_check_margin", 1.0); - p.pull_out_finish_judge_buffer = dp("pull_out_finish_judge_buffer", 1.0); + p.collision_check_distance_from_end = dp("collision_check_distance_from_end", 3.0); // shift pull out p.enable_shift_pull_out = dp("enable_shift_pull_out", true); p.shift_pull_out_velocity = dp("shift_pull_out_velocity", 8.3); p.pull_out_sampling_num = dp("pull_out_sampling_num", 4); - p.before_pull_out_straight_distance = dp("before_pull_out_straight_distance", 3.0); p.minimum_shift_pull_out_distance = dp("minimum_shift_pull_out_distance", 20.0); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); @@ -564,19 +599,21 @@ bool BehaviorPathPlannerNode::isDataReady() { const auto missing = [this](const auto & name) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name); - mutex_pd_.unlock(); return false; }; - mutex_pd_.lock(); // for planner_data_ if (!current_scenario_) { return missing("scenario_topic"); } - if (!planner_data_->route_handler->isHandlerReady()) { + if (!route_ptr_) { return missing("route"); } + if (!map_ptr_) { + return missing("map"); + } + if (!planner_data_->dynamic_object) { return missing("dynamic_object"); } @@ -594,10 +631,39 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_pose"); } - mutex_pd_.unlock(); return true; } +std::shared_ptr BehaviorPathPlannerNode::createLatestPlannerData() +{ + const std::lock_guard lock(mutex_pd_); + + // update self + planner_data_->self_pose = self_pose_listener_.getCurrentPose(); + + // update map + if (has_received_map_) { + planner_data_->route_handler->setMap(*map_ptr_); + has_received_map_ = false; + } + + // update route + const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); + if (has_received_route_) { + planner_data_->route_handler->setRoute(*route_ptr_); + // Reset behavior tree when new route is received, + // so that the each modules do not have to care about the "route jump". + if (!is_first_time) { + RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); + bt_manager_->resetBehaviorTree(); + } + + has_received_route_ = false; + } + + return std::make_shared(*planner_data_); +} + void BehaviorPathPlannerNode::run() { if (!isDataReady()) { @@ -606,19 +672,16 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----"); mutex_bt_.lock(); // for bt_manager_ - mutex_pd_.lock(); // for planner_data_ // behavior_path_planner runs only in LANE DRIVING scenario. if (current_scenario_->current_scenario != Scenario::LANEDRIVING) { mutex_bt_.unlock(); // for bt_manager_ - mutex_pd_.unlock(); // for planner_data_ return; } - // update planner data - planner_data_->self_pose = self_pose_listener_.getCurrentPose(); + // create latest planner data + const auto planner_data = createLatestPlannerData(); - const auto planner_data = planner_data_; // run behavior planner const auto output = bt_manager_->run(planner_data); @@ -631,9 +694,6 @@ void BehaviorPathPlannerNode::run() // compute turn signal computeTurnSignal(planner_data, *path, output); - // unlock planner data - mutex_pd_.unlock(); - // publish drivable bounds publish_bounds(*path); @@ -647,15 +707,14 @@ void BehaviorPathPlannerNode::run() get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); } - const auto path_candidate = getPathCandidate(output, planner_data); - path_candidate_publisher_->publish(util::toPath(*path_candidate)); + publishPathCandidate(bt_manager_->getSceneModules()); publishSceneModuleDebugMsg(); - if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray( - util::getDrivableAreaForAllSharedLinestringLanelets(planner_data)); - debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines); + if (planner_data->parameters.visualize_maximum_drivable_area) { + const auto maximum_drivable_area = + marker_utils::createFurthestLineStringMarkerArray(util::getMaximumDrivableArea(planner_data)); + debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); } mutex_bt_.unlock(); @@ -715,9 +774,9 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path) { - constexpr double scale_x = 0.1; - constexpr double scale_y = 0.1; - constexpr double scale_z = 0.1; + constexpr double scale_x = 0.2; + constexpr double scale_y = 0.2; + constexpr double scale_z = 0.2; constexpr double color_r = 0.0 / 256.0; constexpr double color_g = 148.0 / 256.0; constexpr double color_b = 205.0 / 256.0; @@ -762,6 +821,42 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() } } +void BehaviorPathPlannerNode::publishPathCandidate( + const std::vector> & scene_modules) +{ + for (auto & module : scene_modules) { + if (path_candidate_publishers_.count(module->name()) != 0) { + path_candidate_publishers_.at(module->name()) + ->publish(convertToPath(module->getPathCandidate(), module->isExecutionReady())); + } + } +} + +Path BehaviorPathPlannerNode::convertToPath( + const std::shared_ptr & path_candidate_ptr, const bool is_ready) +{ + Path output; + output.header = planner_data_->route_handler->getRouteHeader(); + output.header.stamp = this->now(); + + if (!path_candidate_ptr) { + return output; + } + + output = util::toPath(*path_candidate_ptr); + // header is replaced by the input one, so it is substituted again + output.header = planner_data_->route_handler->getRouteHeader(); + output.header.stamp = this->now(); + + if (!is_ready) { + for (auto & point : output.points) { + point.longitudinal_velocity_mps = 0.0; + } + } + + return output; +} + PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) { @@ -781,52 +876,29 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( connected_path = modifyPathForSmoothGoalConnection(*path); } - const auto resampled_path = - util::resamplePathWithSpline(connected_path, planner_data_->parameters.path_interval); + const auto resampled_path = util::resamplePathWithSpline( + connected_path, planner_data_->parameters.path_interval, + keepInputPoints(module_status_ptr_vec)); return std::make_shared(resampled_path); } -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( - const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) +bool BehaviorPathPlannerNode::skipSmoothGoalConnection( + const std::vector> & statuses) const { - auto path_candidate = - bt_output.path_candidate ? bt_output.path_candidate : std::make_shared(); - - if (isForcedCandidatePath()) { - for (auto & path_point : path_candidate->points) { - path_point.point.longitudinal_velocity_mps = 0.0; - } - } - - path_candidate->header = planner_data->route_handler->getRouteHeader(); - path_candidate->header.stamp = this->now(); - RCLCPP_DEBUG( - get_logger(), "BehaviorTreeManager: path candidate is %s.", - bt_output.path_candidate ? "FOUND" : "NOT FOUND"); - return path_candidate; -} + const auto target_module = "PullOver"; -bool BehaviorPathPlannerNode::isForcedCandidatePath() const -{ - const auto & module_status_ptr_vec = bt_manager_->getModulesStatus(); - for (const auto & module_status_ptr : module_status_ptr_vec) { - if (!module_status_ptr) { - continue; - } - if (module_status_ptr->module_name != "LaneChange") { - continue; - } - const auto & is_waiting_approval = module_status_ptr->is_waiting_approval; - const auto & is_execution_ready = module_status_ptr->is_execution_ready; - if (is_waiting_approval && !is_execution_ready) { - return true; + for (auto & status : statuses) { + if (status->is_waiting_approval || status->status == BT::NodeStatus::RUNNING) { + if (target_module == status->module_name) { + return true; + } } - break; } return false; } -bool BehaviorPathPlannerNode::skipSmoothGoalConnection( +// This is a temporary process until motion planning can take the terminal pose into account +bool BehaviorPathPlannerNode::keepInputPoints( const std::vector> & statuses) const { const auto target_module = "PullOver"; @@ -843,34 +915,34 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); + const std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; } void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); + const std::lock_guard lock(mutex_pd_); planner_data_->self_acceleration = msg; } void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); + const std::lock_guard lock(mutex_pd_); planner_data_->dynamic_object = msg; } void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); + const std::lock_guard lock(mutex_pd_); planner_data_->occupancy_grid = msg; } void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); + const std::lock_guard lock(mutex_pd_); planner_data_->approval.is_approved.data = msg->approval; // TODO(wep21): Replace msg stamp after {stamp: now} is implemented in ros2 topic pub planner_data_->approval.is_approved.stamp = this->now(); } void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); + const std::lock_guard lock(mutex_pd_); auto getModuleName = [](PathChangeModuleId module) { if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) { return "ForceLaneChange"; @@ -883,22 +955,15 @@ void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstShare } void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); - planner_data_->route_handler->setMap(*msg); + const std::lock_guard lock(mutex_pd_); + map_ptr_ = msg; + has_received_map_ = true; } void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); - const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); - - planner_data_->route_handler->setRoute(*msg); - - // Reset behavior tree when new route is received, - // so that the each modules do not have to care about the "route jump". - if (!is_first_time) { - RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); - bt_manager_->resetBehaviorTree(); - } + const std::lock_guard lock(mutex_pd_); + route_ptr_ = msg; + has_received_route_ = true; } SetParametersResult BehaviorPathPlannerNode::onSetParam( diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 662a7613669cd..54b9afe354591 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -98,6 +98,8 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr RCLCPP_DEBUG(logger_, "BehaviorPathPlanner::run end status = %s", BT::toStr(res).c_str()); + resetNotRunningModulePathCandidate(); + std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { m->publishDebugMarker(); if (!m->isExecutionRequested()) { @@ -127,6 +129,19 @@ BT::NodeStatus BehaviorTreeManager::checkForceApproval(const std::string & name) return approval.module_name == name ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } +void BehaviorTreeManager::resetNotRunningModulePathCandidate() +{ + const bool is_any_module_running = std::any_of( + scene_modules_.begin(), scene_modules_.end(), + [](const auto & module) { return module->current_state_ == BT::NodeStatus::RUNNING; }); + + for (auto & module : scene_modules_) { + if (is_any_module_running && (module->current_state_ != BT::NodeStatus::RUNNING)) { + module->resetPathCandidate(); + } + } +} + void BehaviorTreeManager::resetBehaviorTree() { bt_tree_.haltTree(); } void BehaviorTreeManager::addGrootMonitoring( diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 20d9cfc0fc3b8..457687a6667d3 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -55,7 +55,8 @@ std::vector calcPathArcLengthArray( /** * @brief resamplePathWithSpline */ -PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points) { if (path.points.size() < 2) { return path; @@ -66,7 +67,7 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv transformed_path.at(i) = path.points.at(i).point; } - constexpr double epsilon = 0.01; + constexpr double epsilon = 0.2; const auto has_almost_same_value = [&](const auto & vec, const auto x) { if (vec.empty()) return false; const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; }; @@ -79,6 +80,9 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv for (size_t i = 0; i < path.points.size(); ++i) { const double s = motion_utils::calcSignedArcLength(transformed_path, 0, i); for (const auto & lane_id : path.points.at(i).lane_ids) { + if (keep_input_points && !has_almost_same_value(s_in, s)) { + s_in.push_back(s); + } if ( std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) != unique_lane_ids.end()) { 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 77e6180f3eb82..89548855f233a 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 @@ -47,9 +47,30 @@ using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; +using tier4_autoware_utils::calcLongitudinalDeviation; +using tier4_autoware_utils::calcYawDeviation; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; using tier4_planning_msgs::msg::AvoidanceDebugFactor; +namespace +{ + +AvoidLine getNonStraightShiftLine(const AvoidLineArray & shift_lines) +{ + for (const auto & sl : shift_lines) { + if (fabs(sl.getRelativeLength()) > 0.01) { + return sl; + } + } + + return {}; +} + +} // namespace + AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) : SceneModuleInterface{name, node}, @@ -146,7 +167,7 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // reference pose const auto reference_pose = getUnshiftedEgoPose(prev_output_); - data.reference_pose = reference_pose.pose; + data.reference_pose = reference_pose; // center line path (output of this function must have size > 1) const auto center_path = calcCenterLinePath(planner_data_, reference_pose); @@ -177,8 +198,8 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // lanelet info data.current_lanelets = util::calcLaneAroundPose( - planner_data_->route_handler, reference_pose.pose, - planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length); + planner_data_->route_handler, reference_pose, planner_data_->parameters.forward_path_length, + planner_data_->parameters.backward_path_length); // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -204,14 +225,22 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // detection area filter // when expanding lanelets, right_offset must be minus. // This is because y axis is positive on the left. - const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - data.current_lanelets, parameters_->detection_area_left_expand_dist, + const auto expanded_lanelets = getTargetLanelets( + planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, parameters_->detection_area_right_expand_dist * (-1.0)); - const auto lane_filtered_objects_index = - util::filterObjectIndicesByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + const auto [object_within_target_lane, object_outside_target_lane] = + util::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + for (const auto & object : object_outside_target_lane.objects) { + ObjectData other_object; + other_object.object = object; + other_object.reason = "OutOfTargetArea"; + data.other_objects.push_back(other_object); + } DEBUG_PRINT("dynamic_objects size = %lu", planner_data_->dynamic_object->objects.size()); - DEBUG_PRINT("lane_filtered_objects size = %lu", lane_filtered_objects_index.size()); + DEBUG_PRINT("lane_filtered_objects size = %lu", object_within_target_lane.objects.size()); // for goal const auto & rh = planner_data_->route_handler; @@ -225,8 +254,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // for filtered objects ObjectDataArray target_objects; std::vector avoidance_debug_msg_array; - for (const auto & i : lane_filtered_objects_index) { - const auto & object = planner_data_->dynamic_object->objects.at(i); + for (const auto & object : object_within_target_lane.objects) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; AvoidanceDebugMsg avoidance_debug_msg; const auto avoidance_debug_array_false_and_push_back = @@ -236,15 +264,17 @@ void AvoidanceModule::fillAvoidanceTargetObjects( avoidance_debug_msg_array.push_back(avoidance_debug_msg); }; + ObjectData object_data; + object_data.object = object; + avoidance_debug_msg.object_id = getUuidStr(object_data); + if (!isTargetObjectType(object)) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + object_data.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; + data.other_objects.push_back(object_data); continue; } - ObjectData object_data; - object_data.object = object; - avoidance_debug_msg.object_id = getUuidStr(object_data); - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; @@ -263,6 +293,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( if (object_data.move_time > parameters_->threshold_time_object_is_moving) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT); + object_data.reason = AvoidanceDebugFactor::MOVING_OBJECT; data.other_objects.push_back(object_data); continue; } @@ -270,11 +301,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // object is behind ego or too far. if (object_data.longitudinal < -parameters_->object_check_backward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; data.other_objects.push_back(object_data); continue; } if (object_data.longitudinal > parameters_->object_check_forward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + object_data.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; data.other_objects.push_back(object_data); continue; } @@ -282,6 +315,16 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // Target object is behind the path goal -> ignore. if (object_data.longitudinal > dist_to_goal) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + data.other_objects.push_back(object_data); + continue; + } + + if ( + object_data.longitudinal + object_data.length / 2 + parameters_->object_check_goal_distance > + dist_to_goal) { + avoidance_debug_array_false_and_push_back("TooNearToGoal"); + object_data.reason = "TooNearToGoal"; data.other_objects.push_back(object_data); continue; } @@ -334,6 +377,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; if (std::abs(object_data.lateral) < parameters_->threshold_distance_object_is_on_center) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + object_data.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; data.other_objects.push_back(object_data); continue; } @@ -365,6 +409,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; data.other_objects.push_back(object_data); continue; } @@ -416,6 +461,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( if (!is_parking_object) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; data.other_objects.push_back(object_data); continue; } @@ -503,6 +549,61 @@ void AvoidanceModule::fillObjectMovingTime(ObjectData & object_data) const } } +void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const +{ + constexpr double AVOIDING_SHIFT_THR = 0.1; + data.avoiding_now = std::abs(getCurrentShift()) > AVOIDING_SHIFT_THR; + + auto path_shifter = path_shifter_; + + /** + * STEP 1 + * Create raw shift points from target object. The lateral margin between the ego and the target + * object varies depending on the relative speed between the ego and the target object. + */ + data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data, debug); + + /** + * STEP 2 + * Modify the raw shift points. (Merging, Trimming) + */ + const auto processed_raw_sp = applyPreProcessToRawShiftLines(data.unapproved_raw_sl, debug); + + /** + * STEP 3 + * Find new shift point + */ + data.unapproved_new_sl = findNewShiftLine(processed_raw_sp, path_shifter); + + /** + * STEP 4 + * If there are new shift points, these shift points are registered in path_shifter. + */ + if (!data.unapproved_new_sl.empty()) { + addNewShiftLines(path_shifter, data.unapproved_new_sl); + } + + /** + * STEP 5 + * Generate avoidance path. + */ + auto candidate_path = generateAvoidancePath(path_shifter); + + /** + * STEP 6 + * Check avoidance path safety. For each target objects and the objects in adjacent lanes, + * check that there is a certain amount of margin in the lateral and longitudinal direction. + */ + data.safe = isSafePath(path_shifter, candidate_path, debug); + data.candidate_path = candidate_path; + + { + debug.output_shift = data.candidate_path.shift_length; + debug.current_raw_shift = data.unapproved_raw_sl; + debug.new_shift_lines = data.unapproved_new_sl; + } +} + /** * updateRegisteredRawShiftLines * @@ -535,16 +636,9 @@ void AvoidanceModule::updateRegisteredRawShiftLines() debug_data_.registered_raw_shift = registered_raw_shift_lines_; } -AvoidLineArray AvoidanceModule::calcShiftLines( +AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_lines, DebugData & debug) const { - /** - * Generate raw_shift_lines (shift length, avoidance start point, end point, return point, etc) - * for each object. These raw shift points are merged below to compute appropriate shift points. - */ - current_raw_shift_lines = calcRawShiftLinesFromObjects(avoidance_data_.target_objects); - debug.current_raw_shift = current_raw_shift_lines; - /** * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. @@ -653,9 +747,14 @@ double AvoidanceModule::getShiftLength( * Calculate the shift points (start/end point, shift length) from the object lateral * and longitudinal positions in the Frenet coordinate. The jerk limit is also considered here. */ -AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const +AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( + const AvoidancePlanningData & data, DebugData & debug) const { - debug_avoidance_initializer_for_shift_line_.clear(); + { + debug_avoidance_initializer_for_shift_line_.clear(); + debug.unavoidable_objects.clear(); + } + const auto prepare_distance = getNominalPrepareDistance(); // To be consistent with changes in the ego position, the current shift length is considered. @@ -670,8 +769,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr AvoidLineArray avoid_lines; std::vector avoidance_debug_msg_array; - avoidance_debug_msg_array.reserve(objects.size()); - for (auto & o : objects) { + avoidance_debug_msg_array.reserve(data.target_objects.size()); + for (auto o : data.target_objects) { AvoidanceDebugMsg avoidance_debug_msg; const auto avoidance_debug_array_false_and_push_back = [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) { @@ -697,6 +796,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr } else { avoidance_debug_array_false_and_push_back( AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); + o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + debug.unavoidable_objects.push_back(o); continue; } } @@ -705,6 +806,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr const auto shift_length = getShiftLength(o, is_object_on_right, avoid_margin); if (isSameDirectionShift(is_object_on_right, shift_length)) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); + o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; + debug.unavoidable_objects.push_back(o); continue; } @@ -732,6 +835,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr // is not zero. avoidance_debug_array_false_and_push_back( AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO); + if (!data.avoiding_now) { + o.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + debug.unavoidable_objects.push_back(o); + } continue; } @@ -742,6 +849,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr avoidance_debug_msg.maximum_jerk = parameters_->max_lateral_jerk; if (required_jerk > parameters_->max_lateral_jerk) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_LARGE_JERK); + if (!data.avoiding_now) { + o.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + debug.unavoidable_objects.push_back(o); + } continue; } } @@ -1744,7 +1855,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end = getEgoPose().pose; + last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; last_sl.end_shift_length = getCurrentBaseShift(); } @@ -1780,7 +1891,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end = getEgoPose().pose; + last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; last_sl.end_shift_length = current_base_shift; } @@ -1885,16 +1996,368 @@ void AvoidanceModule::addReturnShiftLineFromEgo( DEBUG_PRINT("Return Shift is added."); } -double AvoidanceModule::getRightShiftBound() const +bool AvoidanceModule::isSafePath( + const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const { - // TODO(Horibe) write me. Real lane boundary must be considered here. - return -parameters_->max_right_shift_length; + const auto & p = parameters_; + + if (!p->enable_safety_check) { + return true; // if safety check is disabled, it always return safe. + } + + const auto & forward_check_distance = p->object_check_forward_distance; + const auto & backward_check_distance = p->safety_check_backward_distance; + const auto check_lanes = + getAdjacentLane(path_shifter, forward_check_distance, backward_check_distance); + + auto path_with_current_velocity = shifted_path.path; + path_with_current_velocity = util::resamplePathWithSpline(path_with_current_velocity, 0.5); + + const size_t ego_idx = findEgoIndex(path_with_current_velocity.points); + util::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); + + constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h + for (auto & p : path_with_current_velocity.points) { + p.point.longitudinal_velocity_mps = std::max(getEgoSpeed(), MIN_EGO_VEL_IN_PREDICTION); + } + + { + debug_data_.path_with_planned_velocity = path_with_current_velocity; + } + + return isSafePath(path_with_current_velocity, check_lanes, debug); } -double AvoidanceModule::getLeftShiftBound() const +bool AvoidanceModule::isSafePath( + const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, DebugData & debug) const { - // TODO(Horibe) write me. Real lane boundary must be considered here. - return parameters_->max_left_shift_length; + if (path.points.empty()) { + return true; + } + + const auto path_with_time = [&path]() { + std::vector> ret{}; + + float travel_time = 0.0; + ret.emplace_back(path.points.front(), travel_time); + + for (size_t i = 1; i < path.points.size(); ++i) { + const auto & p1 = path.points.at(i - 1); + const auto & p2 = path.points.at(i); + + const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); + const auto ds = calcDistance2d(p1, p2); + + travel_time += ds / v; + + ret.emplace_back(p2, travel_time); + } + + return ret; + }(); + + const auto move_objects = getAdjacentLaneObjects(check_lanes); + + { + debug.unsafe_objects.clear(); + debug.margin_data_array.clear(); + debug.exist_adjacent_objects = !move_objects.empty(); + } + + bool is_safe = true; + for (const auto & p : path_with_time) { + MarginData margin_data{}; + margin_data.pose = getPose(p.first); + + if (p.second > parameters_->safety_check_time_horizon) { + break; + } + + for (const auto & o : move_objects) { + const auto is_enough_margin = isEnoughMargin(p.first, p.second, o, margin_data); + + if (!is_enough_margin) { + debug.unsafe_objects.push_back(o); + } + + is_safe = is_safe && is_enough_margin; + } + + debug.margin_data_array.push_back(margin_data); + } + + return is_safe; +} + +bool AvoidanceModule::isEnoughMargin( + const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, + MarginData & margin_data) const +{ + const auto & common_param = planner_data_->parameters; + const auto & vehicle_width = common_param.vehicle_width; + const auto & base_link2front = common_param.base_link2front; + const auto & base_link2rear = common_param.base_link2rear; + + const auto p_ref = [this, &p_ego]() { + const auto idx = findNearestIndex(avoidance_data_.reference_path.points, getPoint(p_ego)); + return getPose(avoidance_data_.reference_path.points.at(idx)); + }(); + + const auto & v_ego = p_ego.point.longitudinal_velocity_mps; + const auto & v_ego_lon = getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); + const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + + if (!isTargetObjectType(object.object)) { + return true; + } + + // | centerline + // | ^ x + // | +-------+ | + // | | | | + // | | | D1 | D2 D4 + // | | obj |<-->|<---------->|<->| + // | | | D3 | +-------+ + // | | |<----------->| | + // | +-------+ | | | + // | | | ego | + // | | | | + // | | | | + // | | +-------+ + // | y <----+ + // D1: overhang_dist (signed value) + // D2: shift_length (signed value) + // D3: lateral_distance (should be larger than margin that's calculated from relative velocity.) + // D4: vehicle_width (unsigned value) + + const auto reliable_path = std::max_element( + object.object.kinematics.predicted_paths.begin(), + object.object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + if (reliable_path == object.object.kinematics.predicted_paths.end()) { + return true; + } + + const auto p_obj = [&t, &reliable_path]() { + boost::optional ret{boost::none}; + + const auto dt = rclcpp::Duration(reliable_path->time_step).seconds(); + const auto idx = static_cast(std::floor(t / dt)); + const auto res = t - dt * idx; + + if (idx > reliable_path->path.size() - 2) { + return ret; + } + + const auto & p_src = reliable_path->path.at(idx); + const auto & p_dst = reliable_path->path.at(idx + 1); + ret = calcInterpolatedPose(p_src, p_dst, res / dt); + return ret; + }(); + + if (!p_obj) { + return true; + } + + const auto v_obj_lon = getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); + + const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); + const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; + const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); + + if (lateral_distance > lateral_margin) { + return true; + } + + const auto lon_deviation = calcLongitudinalDeviation(getPose(p_ego), p_obj.get().position); + const auto is_front_object = lon_deviation > 0.0; + const auto longitudinal_margin = + getRSSLongitudinalDistance(v_ego_lon, v_obj_lon, is_front_object); + const auto vehicle_offset = is_front_object ? base_link2front : base_link2rear; + const auto longitudinal_distance = + std::abs(lon_deviation) - vehicle_offset - 0.5 * object.object.shape.dimensions.x; + + { + margin_data.pose.orientation = p_ref.orientation; + margin_data.enough_lateral_margin = false; + margin_data.longitudinal_distance = + std::min(margin_data.longitudinal_distance, longitudinal_distance); + margin_data.longitudinal_margin = + std::max(margin_data.longitudinal_margin, longitudinal_margin); + margin_data.vehicle_width = vehicle_width; + margin_data.base_link2front = base_link2front; + margin_data.base_link2rear = base_link2rear; + } + + if (longitudinal_distance > longitudinal_margin) { + return true; + } + + return false; +} + +double AvoidanceModule::getLateralMarginFromVelocity(const double velocity) const +{ + const auto & p = parameters_; + + if (p->col_size < 2 || p->col_size * 2 != p->target_velocity_matrix.size()) { + throw std::logic_error("invalid matrix col size."); + } + + if (velocity < p->target_velocity_matrix.front()) { + return p->target_velocity_matrix.at(p->col_size); + } + + if (velocity > p->target_velocity_matrix.at(p->col_size - 1)) { + return p->target_velocity_matrix.back(); + } + + for (size_t i = 1; i < p->col_size; ++i) { + if (velocity < p->target_velocity_matrix.at(i)) { + const auto v1 = p->target_velocity_matrix.at(i - 1); + const auto v2 = p->target_velocity_matrix.at(i); + const auto m1 = p->target_velocity_matrix.at(i - 1 + p->col_size); + const auto m2 = p->target_velocity_matrix.at(i + p->col_size); + + const auto v_clamp = std::clamp(velocity, v1, v2); + return m1 + (m2 - m1) * (v_clamp - v1) / (v2 - v1); + } + } + + return p->target_velocity_matrix.back(); +} + +double AvoidanceModule::getRSSLongitudinalDistance( + const double v_ego, const double v_obj, const bool is_front_object) const +{ + const auto & accel_for_rss = parameters_->safety_check_accel_for_rss; + const auto & idling_time = parameters_->safety_check_idling_time; + + const auto opposite_lane_vehicle = v_obj < 0.0; + + /** + * object and ego already pass each other. + * ======================================= + * Ego--> + * --------------------------------------- + * <--Obj + * ======================================= + */ + if (!is_front_object && opposite_lane_vehicle) { + return 0.0; + } + + /** + * object drive opposite direction. + * ======================================= + * Ego--> + * --------------------------------------- + * <--Obj + * ======================================= + */ + if (is_front_object && opposite_lane_vehicle) { + return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + + std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) + + std::abs(v_obj) * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + + std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss); + } + + /** + * object is in front of ego, and drive same direction. + * ======================================= + * Ego--> + * --------------------------------------- + * Obj--> + * ======================================= + */ + if (is_front_object && !opposite_lane_vehicle) { + return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + + std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - + std::pow(v_obj, 2.0) / (2.0 * accel_for_rss); + } + + /** + * object is behind ego, and drive same direction. + * ======================================= + * Ego--> + * --------------------------------------- + * Obj--> + * ======================================= + */ + if (!is_front_object && !opposite_lane_vehicle) { + return v_obj * idling_time + 0.5 * accel_for_rss * std::pow(v_obj, 2.0) + + std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - + std::pow(v_ego, 2.0) / (2.0 * accel_for_rss); + } + + return 0.0; +} + +lanelet::ConstLanelets AvoidanceModule::getAdjacentLane( + const PathShifter & path_shifter, const double forward_distance, + const double backward_distance) const +{ + const auto & rh = planner_data_->route_handler; + + bool has_left_shift = false; + bool has_right_shift = false; + + for (const auto & sp : path_shifter.getShiftLines()) { + if (sp.end_shift_length > 0.01) { + has_left_shift = true; + continue; + } + + if (sp.end_shift_length < -0.01) { + has_right_shift = true; + continue; + } + } + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Satoshi Ota) + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); + + lanelet::ConstLanelets check_lanes{}; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (has_left_shift && opt_left_lane) { + check_lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (has_right_shift && opt_right_lane) { + check_lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (has_right_shift && !right_opposite_lanes.empty()) { + check_lanes.push_back(right_opposite_lanes.front()); + } + } + + return check_lanes; +} + +ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( + const lanelet::ConstLanelets & adjacent_lanes) const +{ + ObjectDataArray objects; + for (const auto & o : avoidance_data_.other_objects) { + if (isCentroidWithinLanelets(o.object, adjacent_lanes)) { + objects.push_back(o); + } + } + + return objects; } // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep @@ -2090,7 +2553,7 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted // TODO(Horibe) clean up functions: there is a similar code in util as well. PathWithLaneId AvoidanceModule::calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const + const std::shared_ptr & planner_data, const Pose & pose) const { const auto & p = planner_data->parameters; const auto & route_handler = planner_data->route_handler; @@ -2121,9 +2584,9 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath( p.backward_path_length, longest_dist_to_shift_line, backward_length); const lanelet::ConstLanelets current_lanes = - util::calcLaneAroundPose(route_handler, pose.pose, p.forward_path_length, backward_length); + util::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); centerline_path = util::getCenterLinePath( - *route_handler, current_lanes, pose.pose, backward_length, p.forward_path_length, p); + *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); // for debug: check if the path backward distance is same as the desired length. // { @@ -2217,11 +2680,9 @@ boost::optional AvoidanceModule::calcIntersectionShiftLine( BehaviorModuleOutput AvoidanceModule::plan() { - DEBUG_PRINT("AVOIDANCE plan"); - - const auto shift_lines = calcShiftLines(current_raw_shift_lines_, debug_data_); + const auto & data = avoidance_data_; - const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter_); + resetPathCandidate(); /** * Has new shift point? @@ -2232,26 +2693,20 @@ BehaviorModuleOutput AvoidanceModule::plan() * Yes -> clear WAIT_APPROVAL state. * No -> do nothing. */ - if (new_shift_lines) { - debug_data_.new_shift_lines = *new_shift_lines; - DEBUG_PRINT("new_shift_lines size = %lu", new_shift_lines->size()); - printShiftLines(*new_shift_lines, "new_shift_lines"); - int i = new_shift_lines->size() - 1; - for (; i > 0; i--) { - if (fabs(new_shift_lines->at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } - if (new_shift_lines->at(i).getRelativeLength() > 0.0) { + if (!data.unapproved_new_sl.empty()) { + debug_data_.new_shift_lines = data.unapproved_new_sl; + DEBUG_PRINT("new_shift_lines size = %lu", data.unapproved_new_sl.size()); + printShiftLines(data.unapproved_new_sl, "new_shift_lines"); + + const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + if (sl.getRelativeLength() > 0.0) { removePreviousRTCStatusRight(); - } else if (new_shift_lines->at(i).getRelativeLength() < 0.0) { + } else if (sl.getRelativeLength() < 0.0) { removePreviousRTCStatusLeft(); } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); } - addShiftLineIfApproved(*new_shift_lines); + addShiftLineIfApproved(data.unapproved_new_sl); } else if (isWaitingApproval()) { clearWaitingApproval(); removeCandidateRTCStatus(); @@ -2302,35 +2757,22 @@ BehaviorModuleOutput AvoidanceModule::plan() CandidateOutput AvoidanceModule::planCandidate() const { - DEBUG_PRINT("AVOIDANCE planCandidate start"); - CandidateOutput output; + const auto & data = avoidance_data_; - auto path_shifter = path_shifter_; - auto debug_data = debug_data_; - auto current_raw_shift_lines = current_raw_shift_lines_; + CandidateOutput output; - const auto shift_lines = calcShiftLines(current_raw_shift_lines, debug_data); - const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter); - if (new_shift_lines) { - addNewShiftLines(path_shifter, *new_shift_lines); - } + auto shifted_path = data.candidate_path; - auto shifted_path = generateAvoidancePath(path_shifter); + if (!data.unapproved_new_sl.empty()) { // clip from shift start index for visualize + clipByMinStartIdx(data.unapproved_new_sl, shifted_path.path); - if (new_shift_lines) { // clip from shift start index for visualize - clipByMinStartIdx(*new_shift_lines, shifted_path.path); + const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + const auto sl_front = data.unapproved_new_sl.front(); + const auto sl_back = data.unapproved_new_sl.back(); - int i = new_shift_lines->size() - 1; - for (; i > 0; i--) { - if (fabs(new_shift_lines->at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } - output.lateral_shift = new_shift_lines->at(i).getRelativeLength(); - output.start_distance_to_path_change = new_shift_lines->front().start_longitudinal; - output.finish_distance_to_path_change = new_shift_lines->back().end_longitudinal; + output.lateral_shift = sl.getRelativeLength(); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { @@ -2339,7 +2781,7 @@ CandidateOutput AvoidanceModule::planCandidate() const return SteeringFactor::RIGHT; }); steering_factor_interface_ptr_->updateSteeringFactor( - {new_shift_lines->front().start, new_shift_lines->back().end}, + {sl_front.start, sl_back.end}, {output.start_distance_to_path_change, output.finish_distance_to_path_change}, SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); @@ -2366,7 +2808,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() clearWaitingApproval(); removeCandidateRTCStatus(); } - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); return out; } @@ -2377,23 +2819,19 @@ void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) const size_t prev_size = path_shifter_.getShiftLinesSize(); addNewShiftLines(path_shifter_, shift_lines); + current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + // register original points for consistency registerRawShiftLines(shift_lines); - int i = shift_lines.size() - 1; - for (; i > 0; i--) { - if (fabs(shift_lines.at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } + const auto sl = getNonStraightShiftLine(shift_lines); + const auto sl_front = shift_lines.front(); + const auto sl_back = shift_lines.back(); - if (shift_lines.at(i).getRelativeLength() > 0.0) { - left_shift_array_.push_back({uuid_left_, shift_lines.front().start, shift_lines.back().end}); - } else if (shift_lines.at(i).getRelativeLength() < 0.0) { - right_shift_array_.push_back( - {uuid_right_, shift_lines.front().start, shift_lines.back().end}); + if (sl.getRelativeLength() > 0.0) { + left_shift_array_.push_back({uuid_left_, sl_front.start, sl_back.end}); + } else if (sl.getRelativeLength() < 0.0) { + right_shift_array_.push_back({uuid_right_, sl_front.start, sl_back.end}); } uuid_left_ = generateUUID(); @@ -2446,7 +2884,7 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setShiftLines(future); } -boost::optional AvoidanceModule::findNewShiftLine( +AvoidLineArray AvoidanceModule::findNewShiftLine( const AvoidLineArray & candidates, const PathShifter & shifter) const { (void)shifter; @@ -2528,25 +2966,7 @@ boost::optional AvoidanceModule::findNewShiftLine( return {}; } -double AvoidanceModule::getEgoSpeed() const -{ - return std::abs(planner_data_->self_odometry->twist.twist.linear.x); -} - -double AvoidanceModule::getNominalAvoidanceEgoSpeed() const -{ - return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); -} -double AvoidanceModule::getSharpAvoidanceEgoSpeed() const -{ - return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); -} - -Point AvoidanceModule::getEgoPosition() const { return planner_data_->self_pose->pose.position; } - -PoseStamped AvoidanceModule::getEgoPose() const { return *(planner_data_->self_pose); } - -PoseStamped AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const +Pose AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const { const auto ego_pose = getEgoPose(); @@ -2555,43 +2975,17 @@ PoseStamped AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) } // un-shifted fot current ideal pose - const auto closest = findNearestIndex(prev_path.path.points, ego_pose.pose.position); + const auto closest = findNearestIndex(prev_path.path.points, ego_pose.position); - PoseStamped unshifted_pose = ego_pose; - unshifted_pose.pose.orientation = prev_path.path.points.at(closest).point.pose.orientation; + Pose unshifted_pose = ego_pose; + unshifted_pose.orientation = prev_path.path.points.at(closest).point.pose.orientation; - util::shiftPose(&unshifted_pose.pose, -prev_path.shift_length.at(closest)); - unshifted_pose.pose.orientation = ego_pose.pose.orientation; + util::shiftPose(&unshifted_pose, -prev_path.shift_length.at(closest)); + unshifted_pose.orientation = ego_pose.orientation; return unshifted_pose; } -double AvoidanceModule::getNominalAvoidanceDistance(const double shift_length) const -{ - const auto & p = parameters_; - const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); -} - -double AvoidanceModule::getSharpAvoidanceDistance(const double shift_length) const -{ - const auto & p = parameters_; - const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); -} - -double AvoidanceModule::getNominalPrepareDistance() const -{ - const auto & p = parameters_; - const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. - const auto nominal_distance = std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); - return nominal_distance + epsilon_m; -} - ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) const { DEBUG_PRINT("path_shifter: base shift = %f", getCurrentBaseShift()); @@ -2639,6 +3033,8 @@ void AvoidanceModule::updateData() if (prev_reference_.points.empty()) { prev_reference_ = avoidance_data_.reference_path; } + + fillShiftLine(avoidance_data_, debug_data_); } /* @@ -2773,6 +3169,7 @@ void AvoidanceModule::initVariables() debug_data_ = DebugData(); debug_marker_.markers.clear(); + resetPathCandidate(); registered_raw_shift_lines_ = {}; current_raw_shift_lines_ = {}; original_unique_id = 0; @@ -2879,16 +3276,6 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con return turn_signal_info; } -double AvoidanceModule::getCurrentShift() const -{ - return prev_output_.shift_length.at(findNearestIndex(prev_output_.path.points, getEgoPosition())); -} -double AvoidanceModule::getCurrentLinearShift() const -{ - return prev_linear_shift_path_.shift_length.at( - findNearestIndex(prev_linear_shift_path_.path.points, getEgoPosition())); -} - void AvoidanceModule::setDebugData( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { @@ -2899,9 +3286,13 @@ void AvoidanceModule::setDebugData( using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; + using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; + using marker_utils::avoidance_marker::createPredictedVehiclePositions; using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; + using marker_utils::avoidance_marker::createUnavoidableObjectsMarkerArray; + using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; debug_marker_.markers.clear(); @@ -2920,6 +3311,10 @@ void AvoidanceModule::setDebugData( add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); }; + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); + add(createPredictedVehiclePositions( + debug.path_with_planned_velocity, "predicted_vehicle_positions")); + const auto & path = data.reference_path; add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); @@ -2934,6 +3329,9 @@ void AvoidanceModule::setDebugData( add(createOverhangFurthestLineStringMarkerArray( *debug.farthest_linestring_from_overhang, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); + add(createUnavoidableObjectsMarkerArray(debug.unavoidable_objects, "unavoidable_objects")); + add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); + // parent object info addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index ffe2d0a4b83a2..7e378dda5954a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utilities.hpp" #include +#include #include #include @@ -34,6 +35,7 @@ namespace behavior_path_planner using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::pose2transform; @@ -496,4 +498,65 @@ void generateDrivableArea( } } +double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) +{ + return v * std::cos(calcYawDeviation(p_ref, p_target)); +} + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +lanelet::ConstLanelets getTargetLanelets( + const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, + const double left_offset, const double right_offset) +{ + const auto & rh = planner_data->route_handler; + + lanelet::ConstLanelets target_lanelets{}; + for (const auto & lane : route_lanelets) { + auto l_offset = 0.0; + auto r_offset = 0.0; + + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (opt_left_lane) { + target_lanelets.push_back(opt_left_lane.get()); + } else { + l_offset = left_offset; + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (opt_right_lane) { + target_lanelets.push_back(opt_right_lane.get()); + } else { + r_offset = right_offset; + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (!right_opposite_lanes.empty()) { + target_lanelets.push_back(right_opposite_lanes.front()); + } + + const auto expand_lane = lanelet::utils::getExpandedLanelet(lane, l_offset, r_offset); + target_lanelets.push_back(expand_lane); + } + + return target_lanelets; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index c6ded01021c33..86afb38acca38 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -28,10 +28,12 @@ namespace marker_utils::avoidance_marker using behavior_path_planner::AvoidLine; using behavior_path_planner::util::shiftPose; using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::getPose; using visualization_msgs::msg::Marker; namespace @@ -75,21 +77,58 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { - marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + { + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" + << "lateral: " << object.lateral << " [-]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + + { + marker.id = uuidToInt32(object.object.object_id); + marker.pose.position.z += 2.0; + std::ostringstream string_stream; + string_stream << object.reason; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); + marker.scale = createMarkerScale(0.6, 0.6, 0.6); + marker.ns = ns + "_reason"; + msg.markers.push_back(marker); + } + } + + return msg; +} + +} // namespace + +MarkerArray createEgoStatusMarkerArray( + const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose = p_ego; + + { std::ostringstream string_stream; - string_stream << std::fixed << std::setprecision(2); - string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral: " << object.lateral << " [-]"; + string_stream << std::fixed << std::setprecision(2) << std::boolalpha; + string_stream << "avoid_now:" << data.avoiding_now << "," + << "safe:" << data.safe; marker.text = string_stream.str(); + msg.markers.push_back(marker); } return msg; } -} // namespace - MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w) @@ -140,6 +179,68 @@ MarkerArray createAvoidLineMarkerArray( return msg; } +MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + auto p_marker = createDefaultMarker( + "map", current_time, ns, 0L, Marker::POINTS, createMarkerScale(0.4, 0.4, 0.0), + createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + const auto pushPointMarker = [&](const Pose & p, const double t) { + const auto r = t > 10.0 ? 1.0 : t / 10.0; + p_marker.points.push_back(p.position); + p_marker.colors.push_back(createMarkerColor(r, 1.0 - r, 0.0, 0.999)); + }; + + auto t_marker = createDefaultMarker( + "map", current_time, ns + "_text", 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + const auto pushTextMarker = [&](const Pose & p, const double t, const double d, const double v) { + t_marker.id++; + t_marker.pose = p; + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "t[s]: " << t << "\n" + << "d[m]: " << d << "\n" + << "v[m/s]: " << v; + t_marker.text = string_stream.str(); + msg.markers.push_back(t_marker); + }; + + constexpr double dt_save = 1.0; + double t_save = 0.0; + double t_sum = 0.0; + double d_sum = 0.0; + + if (path.points.empty()) { + return msg; + } + + for (size_t i = 1; i < path.points.size(); ++i) { + const auto & p1 = path.points.at(i - 1); + const auto & p2 = path.points.at(i); + const auto ds = calcDistance2d(p1, p2); + + if (t_save < t_sum + 1e-3) { + pushPointMarker(getPose(p1), t_sum); + pushTextMarker(getPose(p1), t_sum, d_sum, p1.point.longitudinal_velocity_mps); + t_save += dt_save; + } + + const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); + + t_sum += ds / v; + d_sum += ds; + } + + msg.markers.push_back(p_marker); + + return msg; +} + MarkerArray createTargetObjectsMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { @@ -195,6 +296,26 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + return createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); +} + +MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), + createMarkerColor(1.0, 0.0, 1.0, 0.9)), + &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + + return msg; +} + MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index dc9a19e4da7d4..c78da5e8e1769 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -36,6 +36,15 @@ #include #include +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -52,27 +61,11 @@ LaneChangeModule::LaneChangeModule( steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); } -BehaviorModuleOutput LaneChangeModule::run() -{ - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - current_state_ = BT::NodeStatus::RUNNING; - is_activated_ = isActivated(); - auto output = plan(); - - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop - return output; - } - - updateSteeringFactorPtr(output); - - return output; -} - void LaneChangeModule::onEntry() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onEntry"); current_state_ = BT::NodeStatus::SUCCESS; + current_lane_change_state_ = LaneChangeStates::Normal; updateLaneChangeStatus(); } @@ -123,12 +116,19 @@ BT::NodeStatus LaneChangeModule::updateState() return current_state_; } + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + if (isAbortState() && !is_within_current_lane) { + current_state_ = BT::NodeStatus::RUNNING; + return current_state_; + } + if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentSpeedLow()) { + if ((isNearEndOfLane() && isCurrentSpeedLow()) || !is_within_current_lane) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } - // cancel lane change path + current_state_ = BT::NodeStatus::FAILURE; return current_state_; } @@ -143,50 +143,90 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { - auto path = status_.lane_change_path.path; + resetPathCandidate(); + is_activated_ = isActivated(); + PathWithLaneId path = status_.lane_change_path.path; if (!isValidPath(path)) { status_.is_safe = false; return BehaviorModuleOutput{}; } - generateExtendedDrivableArea(path); - if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentSpeedLow()) { - const auto stop_point = util::insertStopPoint(0.1, &path); + if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { + const auto stop_point = util::insertStopPoint(0.1, &path); + } + + if (isAbortState()) { + resetPathIfAbort(); + if (is_activated_) { + path = abort_path_->path; } } + generateExtendedDrivableArea(path); + + prev_approved_path_ = path; + BehaviorModuleOutput output; output.path = std::make_shared(path); updateOutputTurnSignal(output); + updateSteeringFactorPtr(output); + return output; } +void LaneChangeModule::resetPathIfAbort() +{ + if (!is_abort_approval_requested_) { + const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); + if (lateral_shift > 0.0) { + removePreviousRTCStatusRight(); + uuid_right_ = generateUUID(); + } else if (lateral_shift < 0.0) { + removePreviousRTCStatusLeft(); + uuid_left_ = generateUUID(); + } + RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); + is_abort_approval_requested_ = true; + is_abort_path_approved_ = false; + return; + } + + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); + is_abort_path_approved_ = true; + clearWaitingApproval(); + } else { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); + is_abort_path_approved_ = false; + waitApproval(); + } +} + CandidateOutput LaneChangeModule::planCandidate() const { CandidateOutput output; + LaneChangePath selected_path; // Get lane change lanes const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - LaneChangePath selected_path; [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); + if (isAbortState()) { + selected_path = *abort_path_; + } + if (selected_path.path.points.empty()) { return output; } - const auto & start_idx = selected_path.shift_line.start_idx; - const auto & end_idx = selected_path.shift_line.end_idx; - output.path_candidate = selected_path.path; - output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - - selected_path.shifted_path.shift_length.at(start_idx); + output.lateral_shift = lane_change_utils::getLateralShift(selected_path); output.start_distance_to_path_change = motion_utils::calcSignedArcLength( selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( @@ -198,12 +238,21 @@ CandidateOutput LaneChangeModule::planCandidate() const BehaviorModuleOutput LaneChangeModule::planWaitingApproval() { + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + + if (is_within_current_lane) { + prev_approved_path_ = getReferencePath(); + } + BehaviorModuleOutput out; - out.path = std::make_shared(getReferencePath()); + out.path = std::make_shared(prev_approved_path_); + const auto candidate = planCandidate(); - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); waitApproval(); + is_abort_path_approved_ = false; return out; } @@ -412,7 +461,7 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const bool LaneChangeModule::isNearEndOfLane() const { const auto & current_pose = getEgoPose(); - const double threshold = util::calcTotalLaneChangeDistanceWithBuffer(planner_data_->parameters); + const double threshold = util::calcTotalLaneChangeDistance(planner_data_->parameters); return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < threshold; @@ -424,18 +473,12 @@ bool LaneChangeModule::isCurrentSpeedLow() const return util::l2Norm(getEgoTwist().linear) < threshold_ms; } -bool LaneChangeModule::isAbortConditionSatisfied() const +bool LaneChangeModule::isAbortConditionSatisfied() { - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - - const auto & current_lanes = status_.current_lanes; + is_abort_condition_satisfied_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; - // check abort enable flag - if (!parameters_->enable_abort_lane_change) { + if (!parameters_->enable_cancel_lane_change) { return false; } @@ -443,70 +486,70 @@ bool LaneChangeModule::isAbortConditionSatisfied() const return false; } - // find closest lanelet in original lane - lanelet::ConstLanelet closest_lanelet{}; - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - if (!lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet)) { - RCLCPP_ERROR_THROTTLE( - getLogger(), clock, 1000, - "Failed to find closest lane! Lane change aborting function is not working!"); - return false; - } - - // check if lane change path is still safe - const bool is_path_safe = std::invoke([this, &route_handler, &dynamic_objects, ¤t_lanes, - ¤t_pose, ¤t_twist, &common_parameters]() { - constexpr double check_distance = 100.0; - // get lanes used for detection - const auto & path = status_.lane_change_path; - const double check_distance_with_path = - check_distance + path.preparation_length + path.lane_change_length; - const auto check_lanes = route_handler->getCheckTargetLanesFromPath( - path.path, status_.lane_change_lanes, check_distance_with_path); - - std::unordered_map debug_data; - - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); - return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data, false, - status_.lane_change_path.acceleration); - }); + Pose ego_pose_before_collision; + const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { - // check vehicle velocity thresh - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; - const bool is_within_original_lane = - lane_change_utils::isEgoWithinOriginalLane(current_lanes, current_pose, common_parameters); - if (is_velocity_low && is_within_original_lane) { + const auto & common_parameters = planner_data_->parameters; + const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), common_parameters); + + if (is_within_original_lane) { + current_lane_change_state_ = LaneChangeStates::Cancel; return true; } - const bool is_distance_small = - lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, *parameters_); + // check abort enable flag + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); + + if (!parameters_->enable_abort_lane_change) { + current_lane_change_state_ = LaneChangeStates::Stop; + return false; + } - // check angle thresh from original lane - const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - closest_lanelet, current_pose, *parameters_); + const auto found_abort_path = lane_change_utils::getAbortPaths( + planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, + *parameters_); - if (is_distance_small && is_angle_diff_small) { + if (!found_abort_path && !is_abort_path_approved_) { + current_lane_change_state_ = LaneChangeStates::Stop; return true; } - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), clock, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to abort! Please be cautious"); + + current_lane_change_state_ = LaneChangeStates::Abort; + + if (!is_abort_path_approved_) { + abort_path_ = std::make_shared(*found_abort_path); + } + + return true; } return false; } +bool LaneChangeModule::isAbortState() const +{ + if (!parameters_->enable_abort_lane_change) { + return false; + } + + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; + } + + if (!abort_path_) { + return false; + } + + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); + return true; +} + bool LaneChangeModule::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -622,6 +665,36 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } +bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const +{ + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & current_lanes = status_.current_lanes; + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; + const auto path = status_.lane_change_path; + + constexpr double check_distance = 100.0; + // get lanes used for detection + const double check_distance_with_path = + check_distance + path.preparation_length + path.lane_change_length; + const auto check_lanes = route_handler->getCheckTargetLanesFromPath( + path.path, status_.lane_change_lanes, check_distance_with_path); + + std::unordered_map debug_data; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + return lane_change_utils::isLaneChangePathSafe( + path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, *parameters_, + common_parameters.expected_front_deceleration_for_abort, + common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, + false, status_.lane_change_path.acceleration); +} + void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) { const auto turn_signal_info = util::getPathTurnSignal( @@ -635,11 +708,17 @@ void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) void LaneChangeModule::resetParameters() { + is_abort_path_approved_ = false; + is_abort_approval_requested_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; + abort_path_ = nullptr; + clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); object_debug_.clear(); debug_marker_.markers.clear(); + resetPathCandidate(); } void LaneChangeModule::acceptVisitor(const std::shared_ptr & visitor) const diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index ffad3231eee05..35d89fc91322e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -14,7 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" @@ -84,14 +86,29 @@ double getDistanceWhenDecelerate( return std::max(distance, minimum_distance); } +std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( + const double velocity, const double shift_length, const double deceleration, + const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, + const LaneChangeParameters & lc_param) +{ + const auto required_time = PathShifter::calcShiftTimeFromJerkAndJerk( + shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); + + const auto lane_changing_average_speed = + std::max(velocity + deceleration * 0.5 * required_time, lc_param.minimum_lane_change_velocity); + const auto expected_dist = lane_changing_average_speed * required_time; + const auto lane_changing_distance = + (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length; + return {lane_changing_average_speed, lane_changing_distance}; +} + std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double & acceleration, const double & prepare_distance, const double & prepare_duration, - const double & prepare_speed, const double & minimum_prepare_distance, - const double & lane_change_distance, const double & lane_changing_duration, - const double & minimum_lane_change_velocity) + const double acceleration, const double prepare_distance, const double prepare_duration, + const double prepare_speed, const double lane_change_distance, const double lane_changing_speed, + const BehaviorPathPlannerParameters & params, const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; path_shifter.setPath(target_lane_reference_path); @@ -100,6 +117,10 @@ std::optional constructCandidatePath( // offset front side bool offset_back = false; + + path_shifter.setVelocity(lane_changing_speed); + path_shifter.setLateralAccelerationLimit(std::abs(lane_change_param.lane_changing_lateral_acc)); + if (!path_shifter.generate(&shifted_path, offset_back)) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), @@ -111,31 +132,30 @@ std::optional constructCandidatePath( candidate_path.preparation_length = prepare_distance; candidate_path.lane_change_length = lane_change_distance; candidate_path.shift_line = shift_line; + candidate_path.reference_lanelets = original_lanelets; + candidate_path.target_lanelets = target_lanelets; const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; const auto lanechange_end_idx = motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); + const auto insertLaneIDs = [](auto & target, const auto src) { + target.lane_ids.insert(target.lane_ids.end(), src.lane_ids.begin(), src.lane_ids.end()); + }; if (lanechange_end_idx) { for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < *lanechange_end_idx) { - point.lane_ids.insert( - point.lane_ids.end(), lane_changing_start_point.lane_ids.begin(), - lane_changing_start_point.lane_ids.end()); - point.lane_ids.insert( - point.lane_ids.end(), lane_changing_end_point.lane_ids.begin(), - lane_changing_end_point.lane_ids.end()); + insertLaneIDs(point, lane_changing_start_point); + insertLaneIDs(point, lane_changing_end_point); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, lane_changing_start_point.point.longitudinal_velocity_mps); continue; } - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); const auto nearest_idx = motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; @@ -151,8 +171,8 @@ std::optional constructCandidatePath( } candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info( - prepare_segment, prepare_speed, minimum_prepare_distance, prepare_duration, shift_line, - shifted_path); + prepare_segment, prepare_speed, params.minimum_lane_change_prepare_distance, prepare_duration, + shift_line, shifted_path); // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { return std::nullopt; @@ -176,32 +196,39 @@ LaneChangePaths getLaneChangePaths( const auto & backward_path_length = common_parameter.backward_path_length; const auto & forward_path_length = common_parameter.forward_path_length; const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; - const auto & lane_changing_duration = parameter.lane_changing_duration; const auto & minimum_lane_change_prepare_distance = common_parameter.minimum_lane_change_prepare_distance; - const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length; const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; const auto & maximum_deceleration = parameter.maximum_deceleration; const auto & lane_change_sampling_num = parameter.lane_change_sampling_num; // get velocity const double current_velocity = util::l2Norm(twist.linear); - - constexpr double buffer = 1.0; // buffer for min_lane_change_length const double acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; - const double target_distance = util::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); + const auto num_lane_change = + std::abs(route_handler.getNumLaneToPreferredLane(original_lanelets.back())); + const auto min_total_lane_changing_dist = + util::calcLaneChangeBuffer(common_parameter, num_lane_change); + + const auto end_of_lane_dist = std::invoke([&]() { + if (route_handler.isInGoalRouteSection(target_lanelets.back())) { + return util::getSignedDistance(pose, route_handler.getGoalPose(), original_lanelets) - + min_total_lane_changing_dist; + } + + return util::getDistanceToEndOfLane(pose, original_lanelets) - min_total_lane_changing_dist; + }); + for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { const double prepare_speed = getExpectedVelocityWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration); - const double lane_changing_speed = - getExpectedVelocityWhenDecelerate(prepare_speed, acceleration, lane_changing_duration); - // skip if velocity becomes less than zero before finishing lane change - if (lane_changing_speed < 0.0) { + // skip if velocity becomes less than zero before starting lane change + if (prepare_speed < 0.0) { continue; } @@ -210,9 +237,6 @@ LaneChangePaths getLaneChangePaths( current_velocity, acceleration, lane_change_prepare_duration, minimum_lane_change_prepare_distance); - const double lane_changing_distance = getDistanceWhenDecelerate( - prepare_speed, acceleration, lane_changing_duration, minimum_lane_change_length); - if (prepare_distance < target_distance) { continue; } @@ -221,10 +245,17 @@ LaneChangePaths getLaneChangePaths( route_handler, original_lanelets, pose, backward_path_length, prepare_distance, lane_change_prepare_duration, minimum_lane_change_velocity); + const auto estimated_shift_length = lanelet::utils::getArcCoordinates( + target_lanelets, prepare_segment_reference.points.front().point.pose); + + const auto [lane_changing_speed, lane_changing_distance] = + calcLaneChangingSpeedAndDistanceWhenDecelerate( + prepare_speed, estimated_shift_length.distance, acceleration, end_of_lane_dist, + common_parameter, parameter); + const PathWithLaneId lane_changing_segment_reference = getLaneChangePathLaneChangingSegment( - route_handler, target_lanelets, pose, forward_path_length, prepare_distance, - lane_changing_distance, minimum_lane_change_length, buffer, lane_changing_duration, - minimum_lane_change_velocity); + route_handler, target_lanelets, pose, prepare_distance, lane_changing_distance, + lane_changing_speed, common_parameter); if ( prepare_segment_reference.points.empty() || lane_changing_segment_reference.points.empty()) { @@ -247,8 +278,8 @@ LaneChangePaths getLaneChangePaths( const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, shift_line, original_lanelets, target_lanelets, acceleration, prepare_distance, - lane_change_prepare_duration, prepare_speed, minimum_lane_change_prepare_distance, - lane_changing_distance, lane_changing_duration, minimum_lane_change_velocity); + lane_change_prepare_duration, prepare_speed, lane_changing_distance, lane_changing_speed, + common_parameter, parameter); if (!candidate_path) { continue; @@ -292,11 +323,13 @@ bool selectSafePath( const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); + Pose ego_pose_before_collision; if (isLaneChangePathSafe( path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, common_parameters, ros_parameters, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, debug_data, true, path.acceleration)) { + common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true, + path.acceleration)) { *selected_path = path; return true; } @@ -355,8 +388,9 @@ bool isLaneChangePathSafe( const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, const double front_decel, - const double rear_decel, std::unordered_map & debug_data, - const bool use_buffer, const double acceleration) + const double rear_decel, Pose & ego_pose_before_collision, + std::unordered_map & debug_data, const bool use_buffer, + const double acceleration) { if (dynamic_objects == nullptr) { return true; @@ -370,12 +404,13 @@ bool isLaneChangePathSafe( const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & enable_collision_check_at_prepare_phase = lane_change_parameters.enable_collision_check_at_prepare_phase; - const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration; - const double check_end_time = lane_change_prepare_duration + lane_changing_duration; - constexpr double ego_predicted_path_min_speed{1.0}; + const auto & lane_changing_safety_check_duration = + lane_change_parameters.lane_changing_safety_check_duration; + const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration; + const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity}; const auto vehicle_predicted_path = util::convertToPredictedPath( path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, - time_resolution, acceleration, ego_predicted_path_min_speed); + time_resolution, acceleration, min_lc_speed); const auto prepare_phase_ignore_target_speed_thresh = lane_change_parameters.prepare_phase_ignore_target_speed_thresh; @@ -383,11 +418,11 @@ bool isLaneChangePathSafe( // find obstacle in lane change target lanes // retrieve lanes that are merging target lanes as well - const auto target_lane_object_indices = - util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); + const auto [target_lane_object_indices, others] = + util::separateObjectIndicesByLanelets(*dynamic_objects, target_lanes); // find objects in current lane - constexpr double check_distance = 100.0; + const double check_distance = common_parameters.forward_path_length; const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); @@ -433,7 +468,7 @@ bool isLaneChangePathSafe( if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, - rear_decel, current_debug_data.second)) { + rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -470,7 +505,7 @@ bool isLaneChangePathSafe( if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, - rear_decel, current_debug_data.second)) { + rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -582,15 +617,14 @@ PathWithLaneId getLaneChangePathPrepareSegment( PathWithLaneId getLaneChangePathLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const Pose & current_pose, const double & forward_path_length, const double & prepare_distance, - const double & lane_change_distance, const double & minimum_lane_change_length, - const double & lane_change_distance_buffer, const double & lane_changing_duration, - const double & minimum_lane_change_velocity) + const Pose & current_pose, const double prepare_distance, const double lane_change_distance, + const double lane_changing_speed, const BehaviorPathPlannerParameters & params) { if (target_lanelets.empty()) { return PathWithLaneId(); } + const auto minimum_lane_change_length = params.minimum_lane_change_length; const ArcCoordinates arc_position = lanelet::utils::getArcCoordinates(target_lanelets, current_pose); const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); @@ -602,21 +636,17 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( return std::min(s_start, lane_length - num * minimum_lane_change_length); }); - const double s_end = std::invoke([&s_start, &forward_path_length, &lane_length, &num, - &minimum_lane_change_length, &lane_change_distance_buffer]() { + const double s_end = std::invoke([&s_start, ¶ms, &lane_length, &num]() { const double s_end = std::min( - s_start + forward_path_length, - lane_length - num * (minimum_lane_change_length + lane_change_distance_buffer)); + s_start + params.forward_path_length, lane_length - util::calcLaneChangeBuffer(params, num)); return std::max(s_end, s_start + std::numeric_limits::epsilon()); }); PathWithLaneId lane_changing_segment = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); for (auto & point : lane_changing_segment.points) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); } return lane_changing_segment; @@ -635,26 +665,6 @@ bool isEgoWithinOriginalLane( lanelet::utils::to2D(lane_poly).basicPolygon()); } -bool isEgoDistanceNearToCenterline( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param) -{ - const auto centerline2d = lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); - lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); - const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); - return distance < lane_change_param.abort_lane_change_distance_thresh; -} - -bool isEgoHeadingAngleLessThanThreshold( - const lanelet::ConstLanelet & closest_lanelet, const Pose & current_pose, - const LaneChangeParameters & lane_change_param) -{ - const double lane_angle = lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); - const double vehicle_yaw = tf2::getYaw(current_pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); - return std::abs(yaw_diff) < lane_change_param.abort_lane_change_angle_thresh; -} - TurnSignalInfo calc_turn_signal_info( const PathWithLaneId & prepare_path, const double prepare_velocity, const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line, @@ -739,4 +749,183 @@ std::vector generateDrivableLanes( return drivable_lanes; } +std::optional getAbortPaths( + const std::shared_ptr & planner_data, const LaneChangePath & selected_path, + [[maybe_unused]] const Pose & ego_pose_before_collision, + const BehaviorPathPlannerParameters & common_param, + [[maybe_unused]] const LaneChangeParameters & lane_change_param) +{ + const auto & route_handler = planner_data->route_handler; + const auto current_speed = util::l2Norm(planner_data->self_odometry->twist.twist.linear); + const auto current_pose = planner_data->self_pose->pose; + const auto reference_lanelets = selected_path.reference_lanelets; + + const auto ego_nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; + const auto ego_nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; + const double minimum_lane_change_length = util::calcLaneChangeBuffer(common_param, 1, 0.0); + + const auto & lane_changing_path = selected_path.path; + const auto lane_changing_end_pose_idx = std::invoke([&]() { + constexpr double s_start = 0.0; + const double s_end = + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length; + + const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + }); + + const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + + const auto get_abort_idx_and_distance = [&](const double param_time) { + double turning_point_dist{0.0}; + if (ego_pose_idx > lane_changing_end_pose_idx) { + return std::make_pair(ego_pose_idx, turning_point_dist); + } + + constexpr auto min_speed{2.77}; + const auto desired_distance = std::max(min_speed, current_speed) * param_time; + const auto & points = lane_changing_path.points; + size_t idx{0}; + for (idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + const auto dist_to_ego = + util::getSignedDistance(current_pose, points.at(idx).point.pose, reference_lanelets); + turning_point_dist = dist_to_ego; + if (dist_to_ego > desired_distance) { + break; + } + } + return std::make_pair(idx, turning_point_dist); + }; + + const auto abort_delta_time = lane_change_param.abort_delta_time; + const auto [abort_start_idx, abort_start_dist] = get_abort_idx_and_distance(abort_delta_time); + const auto [abort_return_idx, abort_return_dist] = + get_abort_idx_and_distance(abort_delta_time * 2); + + if (abort_start_idx >= abort_return_idx) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "abort start idx and return idx is equal. can't compute abort path."); + return std::nullopt; + } + + if (!hasEnoughDistanceToLaneChangeAfterAbort( + *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "insufficient distance to abort."); + return std::nullopt; + } + + const auto abort_start_pose = lane_changing_path.points.at(abort_start_idx).point.pose; + const auto abort_return_pose = lane_changing_path.points.at(abort_return_idx).point.pose; + const auto arc_position = + lanelet::utils::getArcCoordinates(reference_lanelets, abort_return_pose); + const PathWithLaneId reference_lane_segment = std::invoke([&]() { + const double minimum_lane_change_length = + common_param.backward_length_buffer_for_end_of_lane + common_param.minimum_lane_change_length; + + const double s_start = arc_position.length; + double s_end = + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length; + + if (route_handler->isInGoalRouteSection(selected_path.target_lanelets.back())) { + const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates( + selected_path.target_lanelets, route_handler->getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length) - minimum_lane_change_length; + } + PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); + ref.points.back().point.longitudinal_velocity_mps = std::min( + ref.points.back().point.longitudinal_velocity_mps, + static_cast(lane_change_param.minimum_lane_change_velocity)); + return ref; + }); + + ShiftLine shift_line; + shift_line.start = abort_start_pose; + shift_line.end = abort_return_pose; + shift_line.end_shift_length = -arc_position.distance; + shift_line.start_idx = abort_start_idx; + shift_line.end_idx = abort_return_idx; + + PathShifter path_shifter; + path_shifter.setPath(lane_changing_path); + path_shifter.addShiftLine(shift_line); + const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( + shift_line.end_shift_length, abort_start_dist, current_speed); + + if (lateral_jerk > lane_change_param.abort_max_lateral_jerk) { + return std::nullopt; + } + + ShiftedPath shifted_path; + // offset front side + // bool offset_back = false; + if (!path_shifter.generate(&shifted_path)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "failed to generate abort shifted path."); + } + + PathWithLaneId start_to_abort_return_pose; + start_to_abort_return_pose.points.insert( + start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + start_to_abort_return_pose.points.insert( + start_to_abort_return_pose.points.end(), reference_lane_segment.points.begin(), + reference_lane_segment.points.end()); + + auto abort_path = selected_path; + abort_path.shifted_path = shifted_path; + abort_path.path = start_to_abort_return_pose; + abort_path.shift_line = shift_line; + return std::optional{abort_path}; +} + +double getLateralShift(const LaneChangePath & path) +{ + const auto start_idx = path.shift_line.start_idx; + const auto end_idx = path.shift_line.end_idx; + + return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); +} + +bool hasEnoughDistanceToLaneChangeAfterAbort( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const Pose & current_pose, const double abort_return_dist, + const BehaviorPathPlannerParameters & common_param) +{ + const auto minimum_lane_change_distance = common_param.minimum_lane_change_prepare_distance + + common_param.minimum_lane_change_length + + common_param.backward_length_buffer_for_end_of_lane; + const auto abort_plus_lane_change_distance = abort_return_dist + minimum_lane_change_distance; + if (abort_plus_lane_change_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { + return false; + } + + if ( + abort_plus_lane_change_distance > + util::getDistanceToNextIntersection(current_pose, current_lanes)) { + return false; + } + + if ( + route_handler.isInGoalRouteSection(current_lanes.back()) && + abort_plus_lane_change_distance > + util::getSignedDistance(current_pose, route_handler.getGoalPose(), current_lanes)) { + return false; + } + + if ( + abort_plus_lane_change_distance > + util::getDistanceToCrosswalk( + current_pose, current_lanes, *route_handler.getOverallGraphPtr())) { + return false; + } + + return true; +} } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index 76ccead98268e..a080eefdf64f7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -52,8 +52,8 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with objects in shoulder lanes const auto arc_path = planner_.getArcPath(); - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); if (util::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { return {}; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 5ebbf2e415470..79c1a84fafce3 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -31,6 +31,7 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; + namespace behavior_path_planner { PullOutModule::PullOutModule( @@ -100,6 +101,7 @@ void PullOutModule::onExit() clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); + resetPathCandidate(); current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit"); } @@ -165,12 +167,14 @@ BehaviorModuleOutput PullOutModule::plan() { if (isWaitingApproval()) { clearWaitingApproval(); + resetPathCandidate(); // save current_pose when approved for start_point of turn_signal for backward driving last_approved_pose_ = std::make_unique(planner_data_->self_pose->pose); } BehaviorModuleOutput output; if (!status_.is_safe) { + RCLCPP_INFO(getLogger(), "not found safe path"); return output; } @@ -295,7 +299,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() output.path = std::make_shared(stop_path); output.turn_signal_info = calcTurnSignalInfo(); - output.path_candidate = std::make_shared(candidate_path); + path_candidate_ = std::make_shared(candidate_path); const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -368,6 +372,7 @@ void PullOutModule::planWithPriorityOnEfficientPath( const std::vector & start_pose_candidates, const Pose & goal_pose) { status_.is_safe = false; + status_.planner_type = PlannerType::NONE; // plan with each planner for (const auto & planner : pull_out_planners_) { @@ -399,6 +404,7 @@ void PullOutModule::planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose) { status_.is_safe = false; + status_.planner_type = PlannerType::NONE; for (size_t i = 0; i < start_pose_candidates.size(); ++i) { // pull out start pose is current_pose @@ -426,6 +432,33 @@ void PullOutModule::planWithPriorityOnShortBackDistance( } } +void PullOutModule::generateStopPath() +{ + const auto & current_pose = planner_data_->self_pose->pose; + constexpr double dummy_path_distance = 1.0; + const auto & moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); + + // convert Pose to PathPointWithLaneId with 0 velocity. + auto toPathPointWithLaneId = [this](const Pose & pose) { + PathPointWithLaneId p{}; + p.point.pose = pose; + p.point.longitudinal_velocity_mps = 0.0; + lanelet::Lanelet closest_shoulder_lanelet; + lanelet::utils::query::getClosestLanelet( + planner_data_->route_handler->getShoulderLanelets(), pose, &closest_shoulder_lanelet); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + return p; + }; + + PathWithLaneId path{}; + path.points.push_back(toPathPointWithLaneId(current_pose)); + path.points.push_back(toPathPointWithLaneId(moved_pose)); + + status_.pull_out_path.partial_paths.push_back(path); + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; +} + void PullOutModule::updatePullOutStatus() { const auto & route_handler = planner_data_->route_handler; @@ -460,12 +493,14 @@ void PullOutModule::updatePullOutStatus() getLogger(), "search_priority should be efficient_path or short_back_distance, but %s is given.", parameters_.search_priority.c_str()); + throw std::domain_error("[pull_out] invalid search_priority"); } if (!status_.is_safe) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Not found safe pull out path"); - status_.is_safe = false; - return; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); + status_.back_finished = true; // no need to drive backward + generateStopPath(); } checkBackFinished(); @@ -548,8 +583,7 @@ bool PullOutModule::hasFinishedPullOut() const lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); // has passed pull out end point - return arclength_current.length - arclength_pull_out_end.length > - parameters_.pull_out_finish_judge_buffer; + return arclength_current.length - arclength_pull_out_end.length > 0.0; } void PullOutModule::checkBackFinished() @@ -631,7 +665,7 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo() const // pull out path does not overlap const double distance_from_end = motion_utils::calcSignedArcLength( path.points, status_.pull_out_path.end_pose.position, current_pose.position); - if (distance_from_end < parameters_.pull_out_finish_judge_buffer) { + if (distance_from_end < 0.0) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index b72558260ed75..385bbe6486edd 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -47,10 +47,6 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) return boost::none; } - const auto drivable_lanes = - util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto lanes = util::transformToLanelets(drivable_lanes); - // find candidate paths auto pull_out_paths = calcPullOutPaths( *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters, @@ -60,8 +56,8 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } // extract objects in shoulder lane for collision check - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*dynamic_objects, shoulder_lanes); // get safe path for (auto & pull_out_path : pull_out_paths) { @@ -71,15 +67,33 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check lane_departure and collision with path between pull_out_start to pull_out_end PathWithLaneId path_start_to_end{}; { - const auto pull_out_start_idx = findNearestIndex(shift_path.points, start_pose); - const auto pull_out_end_idx = findNearestIndex(shift_path.points, pull_out_path.end_pose); + const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); + + // calculate collision check end idx + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + shift_path.points, pull_out_path.end_pose.position, + parameters_.collision_check_distance_from_end); + + if (collision_check_end_pose) { + return findNearestIndex(shift_path.points, collision_check_end_pose->position); + } else { + return shift_path.points.size() - 1; + } + }); path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + *pull_out_start_idx, - shift_path.points.begin() + *pull_out_end_idx + 1); + path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + collision_check_end_idx + 1); } // check lane departure - if (lane_departure_checker_->checkPathWillLeaveLane(lanes, path_start_to_end)) { + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_->checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), path_start_to_end)) { continue; } @@ -108,7 +122,7 @@ std::vector ShiftPullOut::calcPullOutPaths( const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter) { - std::vector candidate_paths; + std::vector candidate_paths{}; if (road_lanes.empty() || shoulder_lanes.empty()) { return candidate_paths; @@ -117,7 +131,6 @@ std::vector ShiftPullOut::calcPullOutPaths( // rename parameter const double backward_path_length = common_parameter.backward_path_length; const double shift_pull_out_velocity = parameter.shift_pull_out_velocity; - const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; @@ -127,64 +140,59 @@ std::vector ShiftPullOut::calcPullOutPaths( for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; lateral_jerk += jerk_resolution) { - PathShifter path_shifter; - const double distance_to_road_center = getArcCoordinates(road_lanes, start_pose).distance; + // lateral distance from road center to start pose + const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + + PathWithLaneId road_lane_reference_path{}; + { + const auto arc_position = getArcCoordinates(road_lanes, start_pose); + const double s_start = std::max(arc_position.length - backward_path_length, 0.0); + const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); + double s_end = arc_position_goal.length; + road_lane_reference_path = util::resamplePathWithSpline( + route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); + } + PathShifter path_shifter{}; + path_shifter.setPath(road_lane_reference_path); + + // calculate after/before shifted pull out distance const double pull_out_distance = std::max( - path_shifter.calcLongitudinalDistFromJerk( - abs(distance_to_road_center), lateral_jerk, shift_pull_out_velocity), + PathShifter::calcLongitudinalDistFromJerk( + abs(shift_length), lateral_jerk, shift_pull_out_velocity), minimum_shift_pull_out_distance); + const size_t shift_start_idx = + findNearestIndex(road_lane_reference_path.points, start_pose.position); + PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path; + road_lane_reference_path_from_ego.points.erase( + road_lane_reference_path_from_ego.points.begin(), + road_lane_reference_path_from_ego.points.begin() + shift_start_idx); + // before means distance on road lane + const double before_shifted_pull_out_distance = calcBeforeShiftedArcLength( + road_lane_reference_path_from_ego, pull_out_distance, shift_length); // check has enough distance - const double pull_out_total_distance = before_pull_out_straight_distance + pull_out_distance; const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); if (!hasEnoughDistance( - pull_out_total_distance, road_lanes, start_pose, is_in_goal_route_section, goal_pose)) { + before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section, + goal_pose)) { continue; } - PathWithLaneId shoulder_reference_path; - { - const auto arc_position = getArcCoordinates(shoulder_lanes, start_pose); - const double s_start = arc_position.length - backward_path_length; - double s_end = arc_position.length + before_pull_out_straight_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - shoulder_reference_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end); - - // lateral shift to start_pose - for (auto & p : shoulder_reference_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, arc_position.distance, 0); - } - } - - PathWithLaneId road_lane_reference_path; - { - const lanelet::ArcCoordinates arc_position_shift = - getArcCoordinates(road_lanes, shoulder_reference_path.points.back().point.pose); - const lanelet::ArcCoordinates arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - - double s_start = arc_position_shift.length; - double s_end = arc_position_goal.length; - road_lane_reference_path = route_handler.getCenterLinePath(road_lanes, s_start, s_end); - } - path_shifter.setPath(road_lane_reference_path); - - // get shift point start/end - const auto shift_line_start = shoulder_reference_path.points.back(); - const auto shift_line_end = [&]() { + // get shift end pose + const auto shift_end_pose = std::invoke([&]() { const auto arc_position_shift_start = - lanelet::utils::getArcCoordinates(road_lanes, shift_line_start.point.pose); - const double s_start = arc_position_shift_start.length + pull_out_distance; + lanelet::utils::getArcCoordinates(road_lanes, start_pose); + const double s_start = arc_position_shift_start.length + before_shifted_pull_out_distance; const double s_end = s_start + std::numeric_limits::epsilon(); const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); - return path.points.front(); - }(); - - ShiftLine shift_line; - { - shift_line.start = shift_line_start.point.pose; - shift_line.end = shift_line_end.point.pose; - shift_line.end_shift_length = distance_to_road_center; - } + return path.points.front().point.pose; + }); + + // create shift line + ShiftLine shift_line{}; + shift_line.start = start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_length; path_shifter.addShiftLine(shift_line); // offset front side @@ -194,43 +202,28 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } - const auto pull_out_end_idx = - findNearestIndex(shifted_path.path.points, shift_line_end.point.pose); - const auto goal_idx = findNearestIndex(shifted_path.path.points, goal_pose); - - if (pull_out_end_idx && goal_idx) { - // set velocity - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *pull_out_end_idx) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); - continue; - } else if (i > *goal_idx) { - point.point.longitudinal_velocity_mps = 0.0; - continue; - } + // set velocity + const size_t pull_out_end_idx = + findNearestIndex(shifted_path.path.points, shift_end_pose.position); + const size_t goal_idx = findNearestIndex(shifted_path.path.points, goal_pose.position); + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < pull_out_end_idx) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); + continue; + } else if (i >= goal_idx) { + point.point.longitudinal_velocity_mps = 0.0; + continue; } - - PullOutPath candidate_path; - const auto combined_path = combineReferencePath(shoulder_reference_path, shifted_path.path); - candidate_path.partial_paths.push_back(util::resamplePathWithSpline(combined_path, 1.0)); - candidate_path.start_pose = shift_line.start; - candidate_path.end_pose = shift_line.end; - - // add goal pose because resampling removes it - // but this point will be removed by SmoothGoalConnection again - PathPointWithLaneId goal_path_point = shifted_path.path.points.back(); - goal_path_point.point.pose = goal_pose; - goal_path_point.point.longitudinal_velocity_mps = 0.0; - candidate_path.partial_paths.front().points.push_back(goal_path_point); - candidate_paths.push_back(candidate_path); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "lane change end idx not found on target path."); - continue; } + + // add shifted path to candidates + PullOutPath candidate_path; + candidate_path.partial_paths.push_back(shifted_path.path); + candidate_path.start_pose = shift_line.start; + candidate_path.end_pose = shift_line.end; + candidate_paths.push_back(candidate_path); } return candidate_paths; @@ -255,4 +248,26 @@ bool ShiftPullOut::hasEnoughDistance( return true; } +double ShiftPullOut::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + double before_arc_length{0.0}; + double after_arc_length{0.0}; + + for (const auto & [k, segment_length] : motion_utils::calcCurvatureAndArcLength(path.points)) { + // after shifted segment length + const double after_segment_length = + k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + before_arc_length += k < 0 ? offset / (1 - k * dr) : offset * (1 + k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; + } + + return before_arc_length; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp index 4a23860e24e84..da42cd7b3acff 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp @@ -65,22 +65,8 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) // check lane departure with road and shoulder lanes if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; - // collision check - if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { - const bool check_out_of_range = false; - if (occupancy_grid_map_->hasObstacleOnPath(arc_path, check_out_of_range)) { - return {}; - } - } - if (parameters_.use_object_recognition) { - if (util::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, *(planner_data_->dynamic_object), - parameters_.object_recognition_collision_check_margin)) { - return {}; - } - } - PullOverPath pull_over_path{}; + pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index af3abb2d10e48..f8ef93edc55a5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -14,7 +14,9 @@ #include "behavior_path_planner/scene_module/pull_over/goal_searcher.hpp" +#include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/pull_over/util.hpp" +#include "lanelet2_extension/utility/utilities.hpp" #include @@ -24,6 +26,7 @@ namespace behavior_path_planner { using lane_departure_checker::LaneDepartureChecker; +using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; @@ -36,72 +39,108 @@ GoalSearcher::GoalSearcher( { } -std::vector GoalSearcher::search(const Pose & original_goal_pose) +GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) { - std::vector goal_candidates{}; + GoalCandidates goal_candidates{}; - const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); + const auto & route_handler = planner_data_->route_handler; + const double forward_length = parameters_.forward_goal_search_length; + const double backward_length = parameters_.backward_goal_search_length; + const double margin_from_boundary = parameters_.margin_from_boundary; + const double lateral_offset_interval = parameters_.lateral_offset_interval; + const double max_lateral_offset = parameters_.max_lateral_offset; + + const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*route_handler); auto lanes = util::getExtendedCurrentLanes(planner_data_); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + const auto goal_arc_coords = + lanelet::utils::getArcCoordinates(pull_over_lanes, original_goal_pose); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + auto center_line_path = util::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + parameters_.goal_search_interval); - for (double dx = -parameters_.backward_goal_search_length; - dx <= parameters_.forward_goal_search_length; dx += parameters_.goal_search_interval) { - // search goal_pose in lateral direction + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + + std::vector original_search_poses{}; // for search area visualizing + size_t goal_id = 0; + for (const auto & p : center_line_path.points) { + const Pose & center_pose = p.point.pose; + + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( + pull_over_lanes, vehicle_footprint_, center_pose); + if (!distance_from_left_bound) continue; + + const double offset_from_center_line = distance_from_left_bound.value() + margin_from_boundary; + const Pose original_search_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; - bool found_lateral_no_collision_pose = false; + // search goal_pose in lateral direction double lateral_offset = 0.0; - for (double dy = 0; dy <= parameters_.max_lateral_offset; - dy += parameters_.lateral_offset_interval) { + for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_goal_pose, dx, -dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, -dy, 0); - const auto & transformed_vehicle_footprint = + const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); - if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { + + if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { continue; } - if (checkCollision(search_pose)) { + if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { continue; } - // if finding objects near the search pose, - // shift search_pose in lateral direction one more - // because collision may be detected on other path points - if (dy > 0) { - search_pose = calcOffsetPose(search_pose, 0, -parameters_.lateral_offset_interval, 0); - } + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = search_pose; + goal_candidate.lateral_offset = lateral_offset; + goal_candidate.id = goal_id; + goal_id++; + // use longitudinal_distance as distance_from_original_goal + goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( + center_line_path.points, original_goal_pose.position, search_pose.position)); + goal_candidates.push_back(goal_candidate); + } + } + createAreaPolygons(original_search_poses); + + // Sort with distance from original goal + std::sort(goal_candidates.begin(), goal_candidates.end()); + + return goal_candidates; +} + +void GoalSearcher::update(GoalCandidates & goal_candidates) const +{ + // update is_safe + for (auto & goal_candidate : goal_candidates) { + const Pose goal_pose = goal_candidate.goal_pose; - found_lateral_no_collision_pose = true; - break; + // check collision with footprint + if (checkCollision(goal_pose)) { + goal_candidate.is_safe = false; + continue; } - if (!found_lateral_no_collision_pose) continue; + // check margin with pull over lane objects + const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; const auto target_objects = pull_over_utils::filterObjectsByLateralDistance( - search_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, parameters_.object_recognition_collision_check_margin, filter_inside); - if (checkCollisionWithLongitudinalDistance(search_pose, target_objects)) { + if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + goal_candidate.is_safe = false; continue; } - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = search_pose; - goal_candidate.lateral_offset = lateral_offset; - // use longitudinal_distance as distance_from_original_goal - // TODO(kosuke55): use arc length for curve lane - goal_candidate.distance_from_original_goal = - std::abs(inverseTransformPose(search_pose, original_goal_pose).position.x); - - goal_candidates.push_back(goal_candidate); + goal_candidate.is_safe = true; } - // Sort with distance from original goal - std::sort(goal_candidates.begin(), goal_candidates.end()); - - return goal_candidates; } bool GoalSearcher::checkCollision(const Pose & pose) const @@ -169,4 +208,72 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( return false; } +void GoalSearcher::createAreaPolygons(std::vector original_search_poses) +{ + using tier4_autoware_utils::MultiPolygon2d; + using tier4_autoware_utils::Point2d; + using tier4_autoware_utils::Polygon2d; + + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + const double max_lateral_offset = parameters_.max_lateral_offset; + + const auto appendPointToPolygon = + [](Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { + Point2d point{}; + point.x() = geom_point.x; + point.y() = geom_point.y; + boost::geometry::append(polygon.outer(), point); + }; + + boost::geometry::clear(area_polygons_); + for (const auto p : original_search_poses) { + Polygon2d footprint{}; + const Point p_left_front = calcOffsetPose(p, base_link2front, vehicle_width / 2, 0).position; + appendPointToPolygon(footprint, p_left_front); + + const Point p_right_front = + calcOffsetPose(p, base_link2front, -vehicle_width / 2 - max_lateral_offset, 0).position; + appendPointToPolygon(footprint, p_right_front); + + const Point p_right_back = + calcOffsetPose(p, -base_link2rear, -vehicle_width / 2 - max_lateral_offset, 0).position; + appendPointToPolygon(footprint, p_right_back); + + const Point p_left_back = calcOffsetPose(p, -base_link2rear, vehicle_width / 2, 0).position; + appendPointToPolygon(footprint, p_left_back); + + appendPointToPolygon(footprint, p_left_front); + + MultiPolygon2d current_result{}; + boost::geometry::union_(footprint, area_polygons_, current_result); + area_polygons_ = current_result; + } +} + +BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const +{ + BasicPolygons2d area_polygons{}; + for (const auto & ll : lanes) { + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + for (const auto & area : reg_elem->noStoppingAreas()) { + const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); + area_polygons.push_back(area_poly); + } + } + } + return area_polygons; +} + +bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const +{ + for (const auto & area : areas) { + if (boost::geometry::intersects(area, footprint)) { + return true; + } + } + return false; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 29fcf8e12233c..b8ddf7c8e0727 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/util/create_vehicle_footprint.hpp" #include "behavior_path_planner/utilities.hpp" +#include #include #include #include @@ -32,6 +33,7 @@ #include #include +using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using nav_msgs::msg::OccupancyGrid; @@ -86,6 +88,12 @@ PullOverModule::PullOverModule( // for collision check with objects vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + // timer callback for generating candidate paths + const auto period_ns = rclcpp::Rate(1.0).period(); + timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + &node, clock_, period_ns, std::bind(&PullOverModule::onTimer, this), timer_cb_group_); + resetStatus(); } @@ -93,6 +101,9 @@ void PullOverModule::resetStatus() { PUllOverStatus initial_status{}; status_ = initial_status; + pull_over_path_candidates_.clear(); + closest_start_pose_.reset(); + goal_candidates_.clear(); } // This function is needed for waiting for planner_data_ @@ -105,6 +116,70 @@ void PullOverModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +// generate pull over candidate paths +void PullOverModule::onTimer() +{ + // already generated pull over candidate paths + if (!pull_over_path_candidates_.empty()) { + return; + } + + // goals are not yet available. + if (goal_candidates_.empty()) { + return; + } + + // generate valid pull over path candidates and calculate closest start pose + const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); + std::vector path_candidates{}; + std::optional closest_start_pose{}; + double min_start_arc_length = std::numeric_limits::max(); + const auto planCandidatePaths = [&]( + const std::shared_ptr & planner, + const GoalCandidate & goal_candidate) { + planner->setPlannerData(planner_data_); + auto pull_over_path = planner->plan(goal_candidate.goal_pose); + pull_over_path->goal_id = goal_candidate.id; + if (pull_over_path) { + path_candidates.push_back(*pull_over_path); + // calculate closest pull over start pose for stop path + const double start_arc_length = + lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose).length; + if (start_arc_length < min_start_arc_length) { + min_start_arc_length = start_arc_length; + // closest start pose is stop point when not finding safe path + closest_start_pose = pull_over_path->start_pose; + } + } + }; + + // plan candidate paths and set them to the member variable + if (parameters_.search_priority == "efficient_path") { + for (const auto & planner : pull_over_planners_) { + for (const auto & goal_candidate : goal_candidates_) { + planCandidatePaths(planner, goal_candidate); + } + } + } else if (parameters_.search_priority == "close_goal") { + for (const auto & goal_candidate : goal_candidates_) { + for (const auto & planner : pull_over_planners_) { + planCandidatePaths(planner, goal_candidate); + } + } + } else { + RCLCPP_ERROR( + getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", + parameters_.search_priority.c_str()); + throw std::domain_error("[pull_over] invalid search_priority"); + } + + // set member variables + mutex_.lock(); + pull_over_path_candidates_ = path_candidates; + closest_start_pose_ = closest_start_pose; + mutex_.unlock(); +} + BehaviorModuleOutput PullOverModule::run() { current_state_ = BT::NodeStatus::RUNNING; @@ -161,19 +236,22 @@ void PullOverModule::onEntry() // Initialize parallel parking planner status parallel_parking_parameters_ = getGeometricPullOverParameters(); resetStatus(); + + refined_goal_pose_ = calcRefinedGoal(planner_data_->route_handler->getGoalPose()); + if (parameters_.enable_goal_research) { + goal_searcher_->setPlannerData(planner_data_); + goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + } else { + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = refined_goal_pose_; + goal_candidate.distance_from_original_goal = 0.0; + goal_candidates_.push_back(goal_candidate); + } } last_received_time_ = std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); // Use refined goal as modified goal when disabling goal research - refined_goal_pose_ = calcRefinedGoal(); - if (!parameters_.enable_goal_research) { - goal_candidates_.clear(); - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = refined_goal_pose_; - goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); - } } void PullOverModule::onExit() @@ -181,9 +259,9 @@ void PullOverModule::onExit() RCLCPP_DEBUG(getLogger(), "PULL_OVER onExit"); clearWaitingApproval(); removeRTCStatus(); + resetPathCandidate(); steering_factor_interface_ptr_->clearSteeringFactors(); - - // A child node must never return IDLE + debug_marker_.markers.clear(); current_state_ = BT::NodeStatus::SUCCESS; } @@ -245,24 +323,47 @@ bool PullOverModule::isExecutionRequested() const bool PullOverModule::isExecutionReady() const { return true; } -Pose PullOverModule::calcRefinedGoal() const +Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const { - lanelet::ConstLanelet goal_lane; - Pose goal_pose = planner_data_->route_handler->getGoalPose(); - lanelet::Lanelet closest_shoulder_lanelet; lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), goal_pose, &closest_shoulder_lanelet); - const Pose center_pose = - lanelet::utils::getClosestCenterPose(closest_shoulder_lanelet, goal_pose.position); - - const double distance_to_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( - planner_data_->route_handler->getShoulderLanelets(), center_pose); - const double offset_from_center_line = distance_to_left_bound + - planner_data_->parameters.vehicle_width / 2 + - parameters_.margin_from_boundary; - const Pose refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + // calc closest center line pose + Pose center_pose; + { + // find position + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), closest_shoulder_lanelet.centerline()); + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + center_pose.position.x = refined_point.x(); + center_pose.position.y = refined_point.y(); + center_pose.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + center_pose.orientation = tf2::toMsg(tf_quat); + } + + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( + planner_data_->route_handler->getShoulderLanelets(), vehicle_footprint_, center_pose); + if (!distance_from_left_bound) { + RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); + return goal_pose; + } + + const double offset_from_center_line = + distance_from_left_bound.value() + parameters_.margin_from_boundary; + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; } @@ -271,81 +372,15 @@ BT::NodeStatus PullOverModule::updateState() { // pull_out module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. - // if (hasFinishedPullOver()) { - // current_state_ = BT::NodeStatus::SUCCESS; - // return current_state_; - // } - return current_state_; } -bool PullOverModule::isLongEnoughToParkingStart( - const PathWithLaneId & path, const Pose & parking_start_pose) const -{ - const auto dist_to_parking_start_pose = calcSignedArcLength( - path.points, planner_data_->self_pose->pose, parking_start_pose.position, - std::numeric_limits::max(), M_PI_2); - if (!dist_to_parking_start_pose) return false; - - const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - - // once stopped, it cannot start again if start_pose is close. - // so need enough distance to restart - const double eps_vel = 0.01; - // dist to restart should be less than decide_path_distance. - // otherwise, the goal would change immediately after departure. - const double dist_to_restart = parameters_.decide_path_distance / 2; - if (std::abs(current_vel) < eps_vel && *dist_to_parking_start_pose < dist_to_restart) { - return false; - } - - return *dist_to_parking_start_pose > current_to_stop_distance; -} - -bool PullOverModule::planWithEfficientPath() -{ - for (const auto & planner : pull_over_planners_) { - for (const auto & goal_candidate : goal_candidates_) { - planner->setPlannerData(planner_data_); - const auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (!pull_over_path) { - continue; - } - modified_goal_pose_ = goal_candidate.goal_pose; - status_.pull_over_path = *pull_over_path; - status_.planner = planner; - return true; // found safe path - } - } - - return false; // not found safe path -} - -bool PullOverModule::planWithCloseGoal() -{ - for (const auto & goal_candidate : goal_candidates_) { - for (const auto & planner : pull_over_planners_) { - planner->setPlannerData(planner_data_); - const auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (!pull_over_path) { - continue; - } - modified_goal_pose_ = goal_candidate.goal_pose; - status_.pull_over_path = *pull_over_path; - status_.planner = planner; - return true; // found safe path - } - } - - return false; // not found safe path -} - BehaviorModuleOutput PullOverModule::plan() { const auto & current_pose = planner_data_->self_pose->pose; + resetPathCandidate(); + status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); status_.lanes = @@ -362,8 +397,8 @@ BehaviorModuleOutput PullOverModule::plan() } } - // Use decided path if (status_.has_decided_path) { + // Use decided path if (!status_.has_requested_approval) { // request approval again one the final path is decided waitApproval(); @@ -401,69 +436,87 @@ BehaviorModuleOutput PullOverModule::plan() incrementPathIndex(); } } + } else { + // select safe path from pull over path candidates + goal_searcher_->setPlannerData(planner_data_); + mutex_.lock(); + goal_searcher_->update(goal_candidates_); + const auto pull_over_path_candidates = pull_over_path_candidates_; + const auto goal_candidates = goal_candidates_; + mutex_.unlock(); + status_.is_safe = false; + for (const auto & pull_over_path : pull_over_path_candidates) { + // check if goal is safe + const auto goal_candidate_it = std::find_if( + goal_candidates.begin(), goal_candidates.end(), + [pull_over_path](const auto & goal_candidate) { + return goal_candidate.id == pull_over_path.goal_id; + }); + if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { + continue; + } - } else { // Replan shift -> arc forward -> arc backward path with each goal candidate. - // Research goal when enabling research and final path has not been decided - if (parameters_.enable_goal_research) { - goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); - } + // check if path is valid and safe + if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { + continue; + } - // plan paths with several goals and planner - if (parameters_.search_priority == "efficient_path") { - status_.is_safe = planWithEfficientPath(); - } else if (parameters_.search_priority == "close_goal") { - status_.is_safe = planWithCloseGoal(); - } else { - RCLCPP_ERROR( - getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", - parameters_.search_priority.c_str()); - throw std::domain_error("[pull_over] invalid search_priority"); + status_.is_safe = true; + status_.pull_over_path = pull_over_path; + modified_goal_pose_ = pull_over_path.getFullPath().points.back().point.pose; + break; } // Decelerate before the minimum shift distance from the goal search area. if (status_.is_safe) { auto & first_path = status_.pull_over_path.partial_paths.front(); - const auto arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); - const Pose search_start_pose = calcOffsetPose( - refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); + const auto search_start_pose = calcLongitudinalOffsetPose( + first_path.points, refined_goal_pose_.position, + -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); + const Pose deceleration_pose = + search_start_pose ? *search_start_pose : first_path.points.front().point.pose; + constexpr double deceleration_buffer = 15.0; first_path = util::setDecelerationVelocity( - first_path, parameters_.pull_over_velocity, search_start_pose, - -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + first_path, parameters_.pull_over_velocity, deceleration_pose, -deceleration_buffer, + parameters_.deceleration_interval); } - } - // generate drivable area for each partial path - for (size_t i = status_.current_path_idx; i < status_.pull_over_path.partial_paths.size(); ++i) { - auto & path = status_.pull_over_path.partial_paths.at(i); - const auto p = planner_data_->parameters; - const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); - const auto lane = util::expandLanelets( - shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); - util::generateDrivableArea(path, lane, p.vehicle_length, planner_data_); + // generate drivable area for each partial path + for (auto & path : status_.pull_over_path.partial_paths) { + const size_t ego_idx = findEgoIndex(path.points); + util::clipPathLength(path, ego_idx, planner_data_->parameters); + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + } } BehaviorModuleOutput output; - // safe: use pull over path if (status_.is_safe) { + // safe: use pull over path + status_.stop_pose.reset(); output.path = std::make_shared(getCurrentPath()); - output.path_candidate = std::make_shared(getFullPath()); + path_candidate_ = std::make_shared(status_.pull_over_path.getFullPath()); } else { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path. Stop in road lane."); - // safe -> not_safe or no prev_stop_path: generate new stop_path + // not safe: use stop_path if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); status_.prev_stop_path = output.path; // set stop path as pull over path PullOverPath pull_over_path{}; status_.pull_over_path = pull_over_path; - status_.pull_over_path.path = *output.path; status_.pull_over_path.partial_paths.push_back(*output.path); - } else { // not_safe -> not_safe: use previous stop path + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); + } else { + // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } } status_.prev_is_safe = status_.is_safe; @@ -520,9 +573,10 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() updateOccupancyGrid(); BehaviorModuleOutput out; plan(); // update status_ - out.path = std::make_shared(getReferencePath()); - out.path_candidate = status_.is_safe ? std::make_shared(getFullPath()) : out.path; - + out.path = std::make_shared(generateStopPath()); + path_candidate_ = status_.is_safe + ? std::make_shared(status_.pull_over_path.getFullPath()) + : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); @@ -547,7 +601,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() std::pair PullOverModule::calcDistanceToPathChange() const { - const auto & full_path = getFullPath(); + const auto & full_path = status_.pull_over_path.getFullPath(); const auto dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_pose->pose, status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); @@ -563,7 +617,7 @@ void PullOverModule::setParameters(const PullOverParameters & parameters) parameters_ = parameters; } -PathWithLaneId PullOverModule::getReferencePath() const +PathWithLaneId PullOverModule::generateStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_pose->pose; @@ -581,64 +635,87 @@ PathWithLaneId PullOverModule::getReferencePath() const auto reference_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); - // if not approved, stop parking start position or goal search start position. - const auto refined_goal_arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); - const Pose search_start_pose = calcOffsetPose( - refined_goal_pose_, -parameters_.backward_goal_search_length, - -refined_goal_arc_coordinates.distance, 0); - const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : search_start_pose; - - // if stop pose is behind current pose, stop as soon as possible + // if not approved stop road lane. + // stop point priority is + // 1. actual start pose + // 2. closest candidate start pose + // 3. search start pose + // (In the case of the curve lane, the position is not aligned due to the + // difference between the outer and inner sides) + // 4. emergency stop + const auto search_start_pose = calcLongitudinalOffsetPose( + reference_path.points, refined_goal_pose_.position, + -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); + if (!status_.is_safe && !closest_start_pose_ && !search_start_pose) { + return generateEmergencyStopPath(); + } + const Pose stop_pose = + status_.is_safe ? status_.pull_over_path.start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose); + + // if stop pose is closer than min_stop_distance, stop as soon as possible const size_t ego_idx = findEgoIndex(reference_path.points); const size_t stop_idx = findFirstNearestSegmentIndexWithSoftConstraints( reference_path.points, stop_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); const double ego_to_stop_distance = calcSignedArcLength( reference_path.points, current_pose.position, ego_idx, stop_pose.position, stop_idx); - if (ego_to_stop_distance < 0.0) { - return generateStopPath(); + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + const double min_stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + if (ego_to_stop_distance < min_stop_distance) { + return generateEmergencyStopPath(); } // slow down for turn signal, insert stop point to stop_pose reference_path = util::setDecelerationVelocityForTurnSignal( reference_path, stop_pose, planner_data_->parameters.turn_signal_search_time); + status_.stop_pose = stop_pose; // slow down before the search area. + constexpr double deceleration_buffer = 15.0; reference_path = util::setDecelerationVelocity( - reference_path, parameters_.pull_over_velocity, search_start_pose, - -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + reference_path, parameters_.pull_over_velocity, *search_start_pose, -deceleration_buffer, + parameters_.deceleration_interval); + // generate drivable area const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); - const auto lanes = util::expandLanelets( + const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); util::generateDrivableArea( - reference_path, lanes, common_parameters.vehicle_length, planner_data_); + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } -PathWithLaneId PullOverModule::generateStopPath() const +PathWithLaneId PullOverModule::generateEmergencyStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_pose->pose; const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + constexpr double eps_vel = 0.01; + // generate stop reference path const auto s_current = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + // calc minimum stop distance under maximum deceleration + const double min_stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + + // set stop point + const auto stop_idx = + motion_utils::insertStopPoint(current_pose, min_stop_distance, stop_path.points); + if (stop_idx) { + status_.stop_pose = stop_path.points.at(*stop_idx).point.pose; + } + // set deceleration velocity const size_t ego_idx = findEgoIndex(stop_path.points); - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - motion_utils::insertStopPoint(current_pose, current_to_stop_distance, stop_path.points); - for (auto & point : stop_path.points) { auto & p = point.point; const size_t target_idx = findFirstNearestSegmentIndexWithSoftConstraints( @@ -646,10 +723,10 @@ PathWithLaneId PullOverModule::generateStopPath() const common_parameters.ego_nearest_yaw_threshold); const double distance_to_target = calcSignedArcLength( stop_path.points, current_pose.position, ego_idx, p.pose.position, target_idx); - if (0.0 < distance_to_target) { + if (0.0 < distance_to_target && eps_vel < current_vel) { p.longitudinal_velocity_mps = std::clamp( static_cast( - current_vel * (current_to_stop_distance - distance_to_target) / current_to_stop_distance), + current_vel * (min_stop_distance - distance_to_target) / min_stop_distance), 0.0f, p.longitudinal_velocity_mps); } else { p.longitudinal_velocity_mps = @@ -657,13 +734,14 @@ PathWithLaneId PullOverModule::generateStopPath() const } } + // generate drivable area const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes); - const auto lanes = util::expandLanelets( + const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - - util::generateDrivableArea(stop_path, lanes, common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + stop_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return stop_path; } @@ -679,58 +757,6 @@ PathWithLaneId PullOverModule::getCurrentPath() const return status_.pull_over_path.partial_paths.at(status_.current_path_idx); } -PathWithLaneId PullOverModule::getFullPath() const -{ - PathWithLaneId path{}; - const auto & paths = status_.pull_over_path.partial_paths; - for (size_t i = 0; i < paths.size(); ++i) { - if (i == 0) { - path.points.insert(path.points.end(), paths.at(i).points.begin(), paths.at(i).points.end()); - } else { - // skip overlapping point - path.points.insert( - path.points.end(), next(paths.at(i).points.begin()), paths.at(i).points.end()); - } - } - path.points = motion_utils::removeOverlapPoints(path.points); - - return path; -} - -double PullOverModule::calcMinimumShiftPathDistance() const -{ - const double maximum_jerk = parameters_.maximum_lateral_jerk; - const double pull_over_velocity = parameters_.pull_over_velocity; - const auto current_pose = planner_data_->self_pose->pose; - const double distance_after_pull_over = parameters_.after_pull_over_straight_distance; - const double distance_before_pull_over = parameters_.before_pull_over_straight_distance; - const auto & route_handler = planner_data_->route_handler; - - double distance_to_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( - route_handler->getShoulderLanelets(), current_pose); - double offset_from_center_line = distance_to_left_bound + - planner_data_->parameters.vehicle_width / 2 + - parameters_.margin_from_boundary; - - // calculate minimum pull over distance at pull over velocity, maximum jerk and side offset - const double pull_over_distance_min = PathShifter::calcLongitudinalDistFromJerk( - abs(offset_from_center_line), maximum_jerk, pull_over_velocity); - const double pull_over_total_distance_min = - distance_after_pull_over + pull_over_distance_min + distance_before_pull_over; - - return pull_over_total_distance_min; -} - -bool PullOverModule::isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer) const -{ - const auto current_pose = planner_data_->self_pose->pose; - const double distance_to_goal = - std::abs(util::getSignedDistance(current_pose, goal_pose, lanelets)); - - return distance_to_goal > calcMinimumShiftPathDistance() + buffer; -} - bool PullOverModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); @@ -781,7 +807,7 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const const auto & current_pose = planner_data_->self_pose->pose; const auto & start_pose = status_.pull_over_path.start_pose; const auto & end_pose = status_.pull_over_path.end_pose; - const auto & full_path = getFullPath(); + const auto full_path = status_.pull_over_path.getFullPath(); // calc TurnIndicatorsCommand { @@ -812,6 +838,7 @@ void PullOverModule::setDebugData() using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createMarkerColor; const auto add = [this](const MarkerArray & added) { @@ -820,17 +847,12 @@ void PullOverModule::setDebugData() if (parameters_.enable_goal_research) { // Visualize pull over areas - const Pose start_pose = - calcOffsetPose(refined_goal_pose_, -parameters_.backward_goal_search_length, 0, 0); - const Pose end_pose = - calcOffsetPose(refined_goal_pose_, parameters_.forward_goal_search_length, 0, 0); const auto header = planner_data_->route_handler->getRouteHeader(); const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const auto p = planner_data_->parameters; - debug_marker_.markers.push_back(pull_over_utils::createPullOverAreaMarker( - start_pose, end_pose, 0, header, p.base_link2front, p.base_link2rear, p.vehicle_width, - color)); + const double z = refined_goal_pose_.position.z; + add(pull_over_utils::createPullOverAreaMarkerArray( + goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates add(pull_over_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); @@ -841,9 +863,70 @@ void PullOverModule::setDebugData() add(createPoseMarkerArray( status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.9, 0.9, 0.3)); - add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray(status_.pull_over_path.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + } + + // Visualize debug poses + const auto & debug_poses = status_.pull_over_path.debug_poses; + for (size_t i = 0; i < debug_poses.size(); ++i) { + add(createPoseMarkerArray( + debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); + } + + // Visualize stop pose + if (status_.stop_pose) { + add(createStopVirtualWallMarker( + *status_.stop_pose, "pull_over", clock_->now(), 0, + planner_data_->parameters.base_link2front)); + } +} + +bool PullOverModule::checkCollision(const PathWithLaneId & path) const +{ + if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { + const bool check_out_of_range = false; + if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { + return true; + } + } + + if (parameters_.use_object_recognition) { + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, path, *(planner_data_->dynamic_object), + parameters_.object_recognition_collision_check_margin)) { + return true; + } + } + + return false; +} + +bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) const +{ + const Pose & current_pose = planner_data_->self_pose->pose; + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + + // once stopped, the vehicle cannot start again if start_pose is close. + // so need enough distance to restart. + // distance to restart should be less than decide_path_distance. + // otherwise, the goal would change immediately after departure. + constexpr double eps_vel = 0.01; + const double distance_to_start = calcSignedArcLength( + pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); + const double distance_to_restart = parameters_.decide_path_distance / 2; + if (std::abs(current_vel) < eps_vel && distance_to_start < distance_to_restart) { + return false; } + + // prevent emergency stop + const double current_to_stop_distance = + std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + if (distance_to_start < current_to_stop_distance) { + return false; + } + + return true; } void PullOverModule::printParkingPositionError() const diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index 7307ab76278af..d322a9613c6cf 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -34,61 +34,25 @@ ShiftPullOver::ShiftPullOver( occupancy_grid_map_{occupancy_grid_map} { } - boost::optional ShiftPullOver::plan(const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; - const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; const double min_jerk = parameters_.minimum_lateral_jerk; const double max_jerk = parameters_.maximum_lateral_jerk; const int pull_over_sampling_num = parameters_.pull_over_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / pull_over_sampling_num; + // get road and shoulder lanes const auto road_lanes = util::getExtendedCurrentLanes(planner_data_); const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } - const auto goal_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - - // calculate the pose whose longitudinal position is shift end - // and whose lateral position is center line - const auto shift_end_shoulder_center_pose = std::invoke([&]() { - const double s_start = std::max( - goal_shoulder_arc_coords.length - after_pull_over_straight_distance, 0.0); // shift end - const double s_end = s_start + std::numeric_limits::epsilon(); - const auto shoulder_lane_path = - route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end, true); - return shoulder_lane_path.points.front().point.pose; - }); - - // calculate shift end pose - const double shoulder_left_bound_to_center_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, shift_end_shoulder_center_pose); - const double shoulder_left_bound_to_goal_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, goal_pose); - const double shoulder_center_to_goal_distance = - -shoulder_left_bound_to_center_distance + shoulder_left_bound_to_goal_distance; - const auto shift_end_pose = tier4_autoware_utils::calcOffsetPose( - shift_end_shoulder_center_pose, 0, shoulder_center_to_goal_distance, 0); - - // calculate lateral distances - lanelet::ConstLanelet goal_closest_road_lane{}; - lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &goal_closest_road_lane); - const auto road_center_pose = - lanelet::utils::getClosestCenterPose(goal_closest_road_lane, goal_pose.position); - const double shoulder_left_bound_to_road_center = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, road_center_pose); - const double road_center_to_goal_distance = - -shoulder_left_bound_to_road_center + shoulder_left_bound_to_goal_distance; - + // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { - const auto pull_over_path = generatePullOverPath( - road_lanes, shoulder_lanes, shift_end_pose, goal_pose, lateral_jerk, - road_center_to_goal_distance, shoulder_center_to_goal_distance, - shoulder_left_bound_to_goal_distance); + const auto pull_over_path = + generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -96,9 +60,8 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) return {}; } -PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( - const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose, - const double pull_over_distance) const +PathWithLaneId ShiftPullOver::generateReferencePath( + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const { const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_pose->pose; @@ -108,14 +71,10 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose); const double s_start = current_road_arc_coords.length - backward_path_length; - const auto shift_end_road_arc_coords = - lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose); - double s_end = shift_end_road_arc_coords.length - pull_over_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); + const double s_end = std::max( + lanelet::utils::getArcCoordinates(road_lanes, end_pose).length, + s_start + std::numeric_limits::epsilon()); auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); - // resample road straight path and shift source path respectively - road_lane_reference_path = - util::resamplePathWithSpline(road_lane_reference_path, resample_interval_); // decelerate velocity linearly to minimum pull over velocity // (or keep original velocity if it is lower than pull over velocity) @@ -133,192 +92,209 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( return road_lane_reference_path; } -PathWithLaneId ShiftPullOver::generateShoulderLaneReferencePath( - const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose, - const Pose & goal_pose, const double shoulder_center_to_goal_distance) const -{ - const auto & route_handler = planner_data_->route_handler; - - const auto shift_start_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, shift_start_pose); - const double s_start = shift_start_shoulder_arc_coords.length; - const auto goal_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - const double s_end = goal_shoulder_arc_coords.length; - auto shoulder_lane_reference_path = - route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end); - // offset to goal line - for (auto & p : shoulder_lane_reference_path.points) { - p.point.pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, shoulder_center_to_goal_distance, 0); - } - shoulder_lane_reference_path = - util::resamplePathWithSpline(shoulder_lane_reference_path, resample_interval_); - - return shoulder_lane_reference_path; -} - boost::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk, - const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance, - const double shoulder_left_bound_to_goal_distance) const + const Pose & goal_pose, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; + const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; + + const Pose shift_end_pose = + tier4_autoware_utils::calcOffsetPose(goal_pose, -after_pull_over_straight_distance, 0, 0); + + // generate road lane reference path to shift end + const auto road_lane_reference_path_to_shift_end = util::resamplePathWithSpline( + generateReferencePath(road_lanes, shift_end_pose), resample_interval_); - // generate road lane reference path and get shift start pose + // calculate shift length + const Pose & shift_end_pose_road_lane = + road_lane_reference_path_to_shift_end.points.back().point.pose; + const double shift_end_road_to_target_distance = + tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + .y; + + // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( - road_center_to_goal_distance, lateral_jerk, pull_over_velocity); - const auto road_lane_reference_path = - generateRoadLaneReferencePath(road_lanes, shift_end_pose, pull_over_distance); - if (road_lane_reference_path.points.empty()) return {}; - const auto shift_start_pose = road_lane_reference_path.points.back().point.pose; - - // generate Shoulder lane reference path and set it path_shifter - const auto shoulder_lane_reference_path = generateShoulderLaneReferencePath( - shoulder_lanes, shift_start_pose, goal_pose, shoulder_center_to_goal_distance); - if (shoulder_lane_reference_path.points.empty()) return {}; + shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); + const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( + road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( + road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + -before_shifted_pull_over_distance); + if (!shift_start_pose) return {}; + + // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(shoulder_lane_reference_path); - - // generate shift_line and set it to path_sifter + path_shifter.setPath(road_lane_reference_path_to_shift_end); ShiftLine shift_line{}; + shift_line.start = *shift_start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_end_road_to_target_distance; + path_shifter.addShiftLine(shift_line); + ShiftedPath shifted_path{}; + const bool offset_back = true; // offset front side from reference path + if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + + // set same orientation, because the reference center line orientation is not same to the + shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation; + + // for debug. result of shift is not equal to the target + const Pose actual_shift_end_pose = shifted_path.path.points.back().point.pose; + + // interpolate between shift end pose to goal pose + std::vector interpolated_poses = + interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); + for (const auto & pose : interpolated_poses) { + PathPointWithLaneId p = shifted_path.path.points.back(); + p.point.pose = pose; + shifted_path.path.points.push_back(p); + } + + // set goal pose with velocity 0 { - shift_line.start = shift_start_pose; - shift_line.end = shift_end_pose; - const double shoulder_left_bound_to_shift_start_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, shift_start_pose); - const double goal_to_shift_start_distance = - shoulder_left_bound_to_shift_start_distance - shoulder_left_bound_to_goal_distance; - shift_line.end_shift_length = goal_to_shift_start_distance; - path_shifter.addShiftLine(shift_line); + PathPointWithLaneId p{}; + p.point.longitudinal_velocity_mps = 0.0; + p.point.pose = goal_pose; + p.lane_ids = shifted_path.path.points.back().lane_ids; + for (const auto & lane : shoulder_lanes) { + p.lane_ids.push_back(lane.id()); + } + shifted_path.path.points.push_back(p); } - // offset front side from reference path - ShiftedPath shifted_path{}; - const bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + // set the same z as the goal + for (auto & p : shifted_path.path.points) { + p.point.pose.position.z = goal_pose.position.z; + } // check lane departure with road and shoulder lanes - lanelet::ConstLanelets lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - if (lane_departure_checker_.checkPathWillLeaveLane(lanes, shifted_path.path)) return {}; - - // check collision - if (!isSafePath(shifted_path.path)) return {}; + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_.checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), shifted_path.path)) { + return {}; + } // set lane_id and velocity to shifted_path - const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose); - lanelet::ConstLanelet target_shoulder_lanelet{}; - lanelet::utils::query::getClosestLanelet( - shoulder_lanes, shifted_path.path.points.back().point.pose, &target_shoulder_lanelet); - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + for (size_t i = path_shifter.getShiftLines().front().start_idx; + i < shifted_path.path.points.size() - 1; ++i) { auto & point = shifted_path.path.points.at(i); + // set velocity + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); + + // add target lanes to points after shift start // add road lane_ids if not found - for (const auto id : road_lane_reference_path.points.back().lane_ids) { + for (const auto id : shifted_path.path.points.back().lane_ids) { if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { point.lane_ids.push_back(id); } } // add shoulder lane_id if not found - if ( - std::find(point.lane_ids.begin(), point.lane_ids.end(), target_shoulder_lanelet.id()) == - point.lane_ids.end()) { - point.lane_ids.push_back(target_shoulder_lanelet.id()); - } - // set velocity - if (i >= *goal_idx) { - // set velocity after goal - point.point.longitudinal_velocity_mps = 0.0; - } else { - point.point.longitudinal_velocity_mps = pull_over_velocity; + for (const auto & lane : shoulder_lanes) { + if ( + std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) == + point.lane_ids.end()) { + point.lane_ids.push_back(lane.id()); + } } } + // set pull over path PullOverPath pull_over_path{}; - pull_over_path.path = - pull_over_utils::combineReferencePath(road_lane_reference_path, shifted_path.path); - // shift path is connected to one, so partial_paths have only one - pull_over_path.partial_paths.push_back(pull_over_path.path); + pull_over_path.type = getPlannerType(); + pull_over_path.partial_paths.push_back(shifted_path.path); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; - - // check enough distance - if (!hasEnoughDistance( - pull_over_path.path, road_lanes, pull_over_path.start_pose, goal_pose, - pull_over_distance)) { - return {}; - } + pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(actual_shift_end_pose); return pull_over_path; } -bool ShiftPullOver::hasEnoughDistance( - const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, - const Pose & goal_pose, const double pull_over_distance) const +double ShiftPullOver::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) { - const auto & current_pose = planner_data_->self_pose->pose; - const auto & common_params = planner_data_->parameters; - const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - - const size_t ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, common_params.ego_nearest_dist_threshold, - common_params.ego_nearest_yaw_threshold); - const size_t start_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, start_pose, common_params.ego_nearest_dist_threshold, - common_params.ego_nearest_yaw_threshold); - const double dist_to_start_pose = motion_utils::calcSignedArcLength( - path.points, current_pose.position, ego_segment_idx, start_pose.position, start_segment_idx); - - // once stopped, it cannot start again if start_pose is close. - // so need enough distance to restart - constexpr double eps_vel = 0.01; - // dist to restart should be less than decide_path_distance. - // otherwise, the goal would change immediately after departure. - const double dist_to_restart = parameters_.decide_path_distance / 2; - if (std::abs(current_vel) < eps_vel && dist_to_start_pose < dist_to_restart) { - return false; - } - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - if (dist_to_start_pose < current_to_stop_distance) { - return false; + // reverse path for checking from the end point + // note that the sign of curvature is also reversed + PathWithLaneId reversed_path{}; + std::reverse_copy( + path.points.begin(), path.points.end(), std::back_inserter(reversed_path.points)); + + double before_arc_length{0.0}; + double after_arc_length{0.0}; + for (const auto & [k, segment_length] : + motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + // after shifted segment length + const double after_segment_length = + k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + before_arc_length += k > 0 ? offset / (1 + k * dr) : offset * (1 - k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; } - const double road_lane_dist_to_goal = dist_to_start_pose + pull_over_distance; - if (road_lane_dist_to_goal > util::getDistanceToEndOfLane(current_pose, road_lanes)) { - return false; - } + return before_arc_length; +} + +// only two points is supported +std::vector ShiftPullOver::splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + const double h = base_s.at(1) - base_s.at(0); - const bool is_in_goal_route_section = - planner_data_->route_handler->isInGoalRouteSection(road_lanes.back()); - if ( - is_in_goal_route_section && - road_lane_dist_to_goal > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { - return false; + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); } - return true; + return res; } -bool ShiftPullOver::isSafePath(const PathWithLaneId & path) const +std::vector ShiftPullOver::interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval) { - if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { - const bool check_out_of_range = false; - if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { - return false; - } + std::vector interpolated_poses{}; // output + + const std::vector base_s{ + 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const std::vector base_x{start_pose.position.x, end_pose.position.x}; + const std::vector base_y{start_pose.position.y, end_pose.position.y}; + std::vector new_s; + + constexpr double eps = 0.3; // prevent overlapping + for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + new_s.push_back(s); } - if (parameters_.use_object_recognition) { - if (util::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path, *(planner_data_->dynamic_object), - parameters_.object_recognition_collision_check_margin)) { - return false; - } + const std::vector interpolated_x = splineTwoPoints( + base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), + std::cos(tf2::getYaw(end_pose.orientation)), new_s); + const std::vector interpolated_y = splineTwoPoints( + base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), + std::sin(tf2::getYaw(end_pose.orientation)), new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + Pose pose{}; + pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + pose.position.x = interpolated_x.at(i); + pose.position.y = interpolated_y.at(i); + pose.position.z = end_pose.position.z; + interpolated_poses.push_back(pose); } - return true; + return interpolated_poses; } - } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index addc437dfba7a..c691208c9415c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -86,30 +86,24 @@ PredictedObjects filterObjectsByLateralDistance( return filtered_objects; } -Marker createPullOverAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id, - const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, - const double vehicle_width, const std_msgs::msg::ColorRGBA & color) +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z) { - Marker marker = createDefaultMarker( - header.frame_id, header.stamp, "pull_over_area", id, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); - - auto p_left_front = calcOffsetPose(end_pose, base_link2front, vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); - - auto p_right_front = calcOffsetPose(end_pose, base_link2front, -vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_right_front.x, p_right_front.y, p_right_front.z)); - - auto p_right_back = calcOffsetPose(start_pose, -base_link2rear, -vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_right_back.x, p_right_back.y, p_right_back.z)); - - auto p_left_back = calcOffsetPose(start_pose, -base_link2rear, vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_left_back.x, p_left_back.y, p_left_back.z)); + MarkerArray marker_array{}; + for (size_t i = 0; i < area_polygons.size(); ++i) { + Marker marker = createDefaultMarker( + header.frame_id, header.stamp, "pull_over_area_" + std::to_string(i), i, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); + const auto & poly = area_polygons.at(i); + for (const auto & p : poly.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z)); + } - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); + marker_array.markers.push_back(marker); + } - return marker; + return marker_array; } MarkerArray createPosesMarkerArray( @@ -149,12 +143,14 @@ MarkerArray createTextsMarkerArray( } MarkerArray createGoalCandidatesMarkerArray( - std::vector & goal_candidates, const std_msgs::msg::ColorRGBA & color) + GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { // convert to pose vector std::vector pose_vector{}; for (const auto & goal_candidate : goal_candidates) { - pose_vector.push_back(goal_candidate.goal_pose); + if (goal_candidate.is_safe) { + pose_vector.push_back(goal_candidate.goal_pose); + } } auto marker_array = createPosesMarkerArray(pose_vector, "goal_candidates", color); diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp index e4da7f72dea18..7f90ad46feff4 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp @@ -68,7 +68,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() // std::exit(EXIT_FAILURE); // TODO(Horibe) do appropriate handing } scene_module_->unlockRTCCommand(); - return BT::NodeStatus::SUCCESS; + return scene_module_->getNodeStatusWhileWaitingApproval(); } while (rclcpp::ok()) { diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 8f07df3f5d57c..c68934d06be7e 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -46,10 +46,14 @@ void SideShiftModule::initVariables() { reference_path_ = std::make_shared(); start_pose_reset_request_ = false; - lateral_offset_ = 0.0; + requested_lateral_offset_ = 0.0; + inserted_lateral_offset_ = 0.0; + inserted_shift_line_ = ShiftLine{}; + shift_status_ = SideShiftStatus{}; prev_output_ = ShiftedPath{}; prev_shift_line_ = ShiftLine{}; path_shifter_ = PathShifter{}; + resetPathCandidate(); } void SideShiftModule::onEntry() @@ -62,7 +66,6 @@ void SideShiftModule::onExit() { // write me... initVariables(); - current_state_ = BT::NodeStatus::SUCCESS; } @@ -79,7 +82,7 @@ bool SideShiftModule::isExecutionRequested() const // If the desired offset has a non-zero value, return true as we want to execute the plan. - const bool has_request = !isAlmostZero(lateral_offset_); + const bool has_request = !isAlmostZero(requested_lateral_offset_); RCLCPP_DEBUG_STREAM( getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request); @@ -114,7 +117,7 @@ BT::NodeStatus SideShiftModule::updateState() const auto last_sp = path_shifter_.getLastShiftLine(); if (last_sp) { const auto length = std::fabs(last_sp.get().end_shift_length); - const auto lateral_offset = std::fabs(lateral_offset_); + const auto lateral_offset = std::fabs(requested_lateral_offset_); const auto offset_diff = lateral_offset - length; if (!isAlmostZero(offset_diff)) { lateral_offset_change_request_ = true; @@ -125,7 +128,7 @@ BT::NodeStatus SideShiftModule::updateState() }(); const bool no_offset_diff = isOffsetDiffAlmostZero; - const bool no_request = isAlmostZero(lateral_offset_); + const bool no_request = isAlmostZero(requested_lateral_offset_); const auto no_shifted_plan = [&]() { if (prev_output_.shift_length.empty()) { @@ -145,6 +148,22 @@ BT::NodeStatus SideShiftModule::updateState() if (no_request && no_shifted_plan && no_offset_diff) { current_state_ = BT::NodeStatus::SUCCESS; } else { + const auto & current_lanes = util::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_pose->pose; + const auto & inserted_shift_line_start_pose = inserted_shift_line_.start; + const auto & inserted_shift_line_end_pose = inserted_shift_line_.end; + const double self_to_shift_line_start_arc_length = + behavior_path_planner::util::getSignedDistance( + current_pose, inserted_shift_line_start_pose, current_lanes); + const double self_to_shift_line_end_arc_length = behavior_path_planner::util::getSignedDistance( + current_pose, inserted_shift_line_end_pose, current_lanes); + if (self_to_shift_line_start_arc_length >= 0) { + shift_status_ = SideShiftStatus::BEFORE_SHIFT; + } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) { + shift_status_ = SideShiftStatus::SHIFTING; + } else { + shift_status_ = SideShiftStatus::AFTER_SHIFT; + } current_state_ = BT::NodeStatus::RUNNING; } @@ -179,103 +198,42 @@ void SideShiftModule::updateData() path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); } -bool SideShiftModule::addShiftLine() +void SideShiftModule::replaceShiftLine() { auto shift_lines = path_shifter_.getShiftLines(); - - const auto calcLongitudinal_to_shift_start = [this](const auto & sp) { - return motion_utils::calcSignedArcLength( - reference_path_->points, getEgoPose().pose.position, sp.start.position); - }; - const auto calcLongitudinal_to_shift_end = [this](const auto & sp) { - return motion_utils::calcSignedArcLength( - reference_path_->points, getEgoPose().pose.position, sp.end.position); - }; - - // remove shift points on a far position. - const auto remove_far_iter = std::remove_if( - shift_lines.begin(), shift_lines.end(), - [this, calcLongitudinal_to_shift_start](const ShiftLine & sp) { - const auto dist_to_start = calcLongitudinal_to_shift_start(sp); - constexpr double max_remove_threshold_time = 1.0; // [s] - constexpr double max_remove_threshold_dist = 2.0; // [m] - const auto ego_current_speed = planner_data_->self_odometry->twist.twist.linear.x; - const auto remove_threshold = - std::max(ego_current_speed * max_remove_threshold_time, max_remove_threshold_dist); - return (dist_to_start > remove_threshold); - }); - - shift_lines.erase(remove_far_iter, shift_lines.end()); - - // check if the new_shift_lines overlap with existing shift points. - const auto new_sp = calcShiftLine(); - // check if the new_shift_lines is same with lately inserted shift_lines. - if (new_sp.end_shift_length == prev_shift_line_.end_shift_length) { - return false; + if (shift_lines.size() > 0) { + shift_lines.clear(); } - const auto new_sp_longitudinal_to_shift_start = calcLongitudinal_to_shift_start(new_sp); - const auto new_sp_longitudinal_to_shift_end = calcLongitudinal_to_shift_end(new_sp); - - const auto remove_overlap_iter = std::remove_if( - shift_lines.begin(), shift_lines.end(), - [this, calcLongitudinal_to_shift_start, calcLongitudinal_to_shift_end, - new_sp_longitudinal_to_shift_start, new_sp_longitudinal_to_shift_end](const ShiftLine & sp) { - const bool check_with_prev_sp = (sp.end_shift_length == prev_shift_line_.end_shift_length); - const auto old_sp_longitudinal_to_shift_start = calcLongitudinal_to_shift_start(sp); - const auto old_sp_longitudinal_to_shift_end = calcLongitudinal_to_shift_end(sp); - const bool sp_overlap_front = - ((new_sp_longitudinal_to_shift_start <= old_sp_longitudinal_to_shift_start) && - (old_sp_longitudinal_to_shift_start <= new_sp_longitudinal_to_shift_end)); - const bool sp_overlap_back = - ((new_sp_longitudinal_to_shift_start <= old_sp_longitudinal_to_shift_end) && - (old_sp_longitudinal_to_shift_end <= new_sp_longitudinal_to_shift_end)); - const bool sp_new_contain_old = - ((new_sp_longitudinal_to_shift_start <= old_sp_longitudinal_to_shift_start) && - (old_sp_longitudinal_to_shift_end <= new_sp_longitudinal_to_shift_end)); - const bool sp_old_contain_new = - ((old_sp_longitudinal_to_shift_start <= new_sp_longitudinal_to_shift_start) && - (new_sp_longitudinal_to_shift_end <= old_sp_longitudinal_to_shift_end)); - const bool overlap_with_new_sp = - (sp_overlap_front || sp_overlap_back || sp_new_contain_old || sp_old_contain_new); - - return (overlap_with_new_sp && !check_with_prev_sp); - }); - - shift_lines.erase(remove_overlap_iter, shift_lines.end()); - - // check if the new_shift_line has conflicts with existing shift points. - for (const auto & sp : shift_lines) { - if (calcLongitudinal_to_shift_start(sp) >= new_sp_longitudinal_to_shift_start) { - RCLCPP_WARN( - getLogger(), - "try to add shift point, but shift point already exists behind the proposed point. " - "Ignore the current proposal."); - return false; - } - } + const auto new_sl = calcShiftLine(); // if no conflict, then add the new point. - shift_lines.push_back(new_sp); - const bool new_sp_is_same_with_previous = - new_sp.end_shift_length == prev_shift_line_.end_shift_length; + shift_lines.push_back(new_sl); + const bool new_sl_is_same_with_previous = + new_sl.end_shift_length == prev_shift_line_.end_shift_length; - if (!new_sp_is_same_with_previous) { - prev_shift_line_ = new_sp; + if (!new_sl_is_same_with_previous) { + prev_shift_line_ = new_sl; } // set to path_shifter path_shifter_.setShiftLines(shift_lines); lateral_offset_change_request_ = false; + inserted_lateral_offset_ = requested_lateral_offset_; + inserted_shift_line_ = new_sl; - return true; + return; } BehaviorModuleOutput SideShiftModule::plan() { - // Update shift point - if (lateral_offset_change_request_) { - addShiftLine(); + // Replace shift line + if ( + lateral_offset_change_request_ && ((shift_status_ == SideShiftStatus::BEFORE_SHIFT) || + (shift_status_ == SideShiftStatus::AFTER_SHIFT))) { + replaceShiftLine(); + } else if (shift_status_ != SideShiftStatus::BEFORE_SHIFT) { + RCLCPP_DEBUG(getLogger(), "ego is shifting"); } else { RCLCPP_DEBUG(getLogger(), "change is not requested"); } @@ -326,7 +284,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() BehaviorModuleOutput output; output.path = std::make_shared(shifted_path.path); - output.path_candidate = std::make_shared(planCandidate().path_candidate); + path_candidate_ = std::make_shared(planCandidate().path_candidate); prev_output_ = shifted_path; @@ -340,11 +298,11 @@ void SideShiftModule::onLateralOffset(const LateralOffset::ConstSharedPtr latera const double new_lateral_offset = lateral_offset_msg->lateral_offset; RCLCPP_DEBUG( - getLogger(), "onLateralOffset start : lateral offset current = %f, new = &%f", lateral_offset_, - new_lateral_offset); + getLogger(), "onLateralOffset start : lateral offset current = %f, new = &%f", + requested_lateral_offset_, new_lateral_offset); // offset is not changed. - if (std::abs(lateral_offset_ - new_lateral_offset) < 1e-4) { + if (std::abs(inserted_lateral_offset_ - new_lateral_offset) < 1e-4) { return; } @@ -355,8 +313,7 @@ void SideShiftModule::onLateralOffset(const LateralOffset::ConstSharedPtr latera // new offset is requested. if (isReadyForNextRequest(parameters_.shift_request_time_limit)) { lateral_offset_change_request_ = true; - - lateral_offset_ = new_lateral_offset; + requested_lateral_offset_ = new_lateral_offset; } } @@ -369,7 +326,7 @@ ShiftLine SideShiftModule::calcShiftLine() const std::max(p.min_distance_to_start_shifting, ego_speed * p.time_to_start_shifting); const double dist_to_end = [&]() { - const double shift_length = lateral_offset_ - getClosestShiftLength(); + const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( shift_length, p.shifting_lateral_jerk, std::max(ego_speed, p.min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p.min_shifting_distance); @@ -382,7 +339,7 @@ ShiftLine SideShiftModule::calcShiftLine() const const size_t nearest_idx = findEgoIndex(reference_path_->points); ShiftLine shift_line; - shift_line.end_shift_length = lateral_offset_; + shift_line.end_shift_length = requested_lateral_offset_; shift_line.start_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_start); shift_line.start = reference_path_->points.at(shift_line.start_idx).point.pose; shift_line.end_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_end); diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 4ce7c253c51be..38cecf8ee699d 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -98,31 +98,6 @@ PathWithLaneId GeometricParallelParking::getArcPath() const bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } -bool GeometricParallelParking::isEnoughDistanceToStart(const Pose & start_pose) const -{ - const Pose current_pose = planner_data_->self_pose->pose; - const Pose current_to_start = - inverseTransformPose(start_pose, current_pose); // todo: arc length is better - - // not enough to stop with max deceleration - const double current_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); - const double stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - if (current_to_start.position.x < stop_distance) { - return false; - } - - // not enough to restart from stopped - constexpr double min_restart_distance = 3.0; - if ( - current_vel < parameters_.th_stopped_velocity && - current_to_start.position.x > parameters_.th_arrived_distance && - current_to_start.position.x < min_restart_distance) { - return false; - } - - return true; -} - void GeometricParallelParking::setVelocityToArcPaths( std::vector & arc_paths, const double velocity) { @@ -143,9 +118,6 @@ std::vector GeometricParallelParking::generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const double end_pose_offset, const double velocity) { - if (!isEnoughDistanceToStart(start_pose)) { - return std::vector{}; - } const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin : parameters_.backward_parking_lane_departure_margin; @@ -163,6 +135,16 @@ std::vector GeometricParallelParking::generatePullOverPaths( // straight path from current to parking start const auto straight_path = generateStraightPath(start_pose); + // check the continuity of straight path and arc path + const Pose & road_path_last_pose = straight_path.points.back().point.pose; + const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; + const double yaw_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_last_pose.orientation), tf2::getYaw(arc_path_first_pose.orientation)); + const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); + if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + return std::vector{}; + } + // combine straight_path -> arc_path*2 auto paths = arc_paths; paths.insert(paths.begin(), straight_path); diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index 2fab91e03365d..efd20453c6c57 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -40,6 +40,14 @@ std::string toStr(const behavior_path_planner::ShiftLine & p) ", end idx = " + std::to_string(p.end_idx) + ", length = " + std::to_string(p.end_shift_length); } +std::string toStr(const std::vector & v) +{ + std::stringstream ss; + for (const auto & p : v) { + ss << p << ", "; + } + return ss.str(); +} } // namespace namespace behavior_path_planner @@ -55,6 +63,10 @@ void PathShifter::setPath(const PathWithLaneId & path) updateShiftLinesIndices(shift_lines_); sortShiftLinesAlongPath(shift_lines_); } +void PathShifter::setVelocity(const double velocity) { velocity_ = velocity; } + +void PathShifter::setLateralAccelerationLimit(const double acc) { acc_limit_ = acc; } + void PathShifter::addShiftLine(const ShiftLine & line) { shift_lines_.push_back(line); @@ -195,12 +207,12 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs // TODO(Watanabe) write docs. // These points are defined to achieve the constant-jerk shifting (see the header description). - const std::vector base_distance = { - 0.0, shifting_arclength / 4.0, shifting_arclength * 3.0 / 4.0, shifting_arclength}; - const auto base_length = - offset_back - ? std::vector{0.0, delta_shift / 12.0, delta_shift * 11.0 / 12.0, delta_shift} - : std::vector{delta_shift, delta_shift * 11.0 / 12.0, delta_shift / 12.0, 0.0}; + const auto [base_distance, base_length] = + calcBaseLengths(shifting_arclength, delta_shift, offset_back); + + RCLCPP_DEBUG( + logger_, "base_distance = %s, base_length = %s", toStr(base_distance).c_str(), + toStr(base_length).c_str()); std::vector query_distance, query_length; @@ -238,6 +250,81 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs } } +std::pair, std::vector> PathShifter::getBaseLengthsWithoutAccelLimit( + const double arclength, const double shift_length, const bool offset_back) const +{ + const auto s = arclength; + const auto l = shift_length; + std::vector base_lon = {0.0, 1.0 / 4.0 * s, 3.0 / 4.0 * s, s}; + std::vector base_lat = {0.0, 1.0 / 12.0 * l, 11.0 / 12.0 * l, l}; + + if (!offset_back) std::reverse(base_lat.begin(), base_lat.end()); + + return std::pair{base_lon, base_lat}; +} + +std::pair, std::vector> PathShifter::calcBaseLengths( + const double arclength, const double shift_length, const bool offset_back) const +{ + const auto speed = std::abs(velocity_); + + if (speed < 1.0e-5) { + // no need to consider acceleration limit + RCLCPP_DEBUG(logger_, "set velocity is zero. acc limit is ignored"); + return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + } + + const auto T = arclength / speed; + const auto L = std::abs(shift_length); + const auto a_max = 8.0 * L / (T * T); + + if (a_max < acc_limit_) { + // no need to consider acceleration limit + RCLCPP_WARN_THROTTLE( + logger_, clock_, 3000, "No need to consider acc limit. max: %f, limit: %f", a_max, + acc_limit_); + return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + } + + const auto tj = T / 2.0 - 2.0 * L / (acc_limit_ * T); + const auto ta = 4.0 * L / (acc_limit_ * T) - T / 2.0; + const auto jerk = (2.0 * acc_limit_ * acc_limit_ * T) / (acc_limit_ * T * T - 4.0 * L); + + if (tj < 0.0 || ta < 0.0 || jerk < 0.0 || tj / T < 0.1) { + // no need to consider acceleration limit + RCLCPP_WARN_THROTTLE( + logger_, clock_, 3000, + "Acc limit is too small to be applied. Tj: %f, Ta: %f, j: %f, a_max: %f, acc_limit: %f", tj, + ta, jerk, a_max, acc_limit_); + return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + } + + const auto tj3 = tj * tj * tj; + const auto ta2_tj = ta * ta * tj; + const auto ta_tj2 = ta * tj * tj; + + const auto s1 = tj * speed; + const auto s2 = s1 + ta * speed; + const auto s3 = s2 + tj * speed; // = s4 + const auto s5 = s3 + tj * speed; + const auto s6 = s5 + ta * speed; + const auto s7 = s6 + tj * speed; + + const auto sign = shift_length > 0.0 ? 1.0 : -1.0; + const auto l1 = sign * (1.0 / 6.0 * jerk * tj3); + const auto l2 = sign * (1.0 / 6.0 * jerk * tj3 + 0.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); + const auto l3 = sign * (jerk * tj3 + 1.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); // = l4 + const auto l5 = sign * (11.0 / 6.0 * jerk * tj3 + 2.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); + const auto l6 = sign * (11.0 / 6.0 * jerk * tj3 + 3.0 * jerk * ta_tj2 + jerk * ta2_tj); + const auto l7 = sign * (2.0 * jerk * tj3 + 3.0 * jerk * ta_tj2 + jerk * ta2_tj); + + std::vector base_lon = {0.0, s1, s2, s3, s5, s6, s7}; + std::vector base_lat = {0.0, l1, l2, l3, l5, l6, l7}; + if (!offset_back) std::reverse(base_lat.begin(), base_lat.end()); + + return {base_lon, base_lat}; +} + std::vector PathShifter::calcLateralJerk() const { const auto arclength_arr = util::calcPathArcLengthArray(reference_path_); @@ -386,4 +473,87 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) const } } +double PathShifter::calcShiftTimeFromJerkAndJerk( + const double lateral, const double jerk, const double acc) +{ + const double j = std::abs(jerk); + const double a = std::abs(acc); + const double l = std::abs(lateral); + if (j < 1.0e-8 || a < 1.0e-8) { + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + + // time with constant jerk + double tj = a / j; + + // time with constant acceleration (zero jerk) + double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); + + if (ta < 0.0) { + // it will not hit the acceleration limit this time + tj = std::pow(l / (2.0 * j), 1.0 / 3.0); + ta = 0.0; + } + + const double t_total = 4.0 * tj + 2.0 * ta; + return t_total; +} + +double PathShifter::calcLongitudinalDistFromJerk( + const double lateral, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; +} + +double PathShifter::calcJerkFromLatLonDistance( + const double lateral, const double longitudinal, const double velocity) +{ + constexpr double ep = 1.0e-3; + const double lat = std::abs(lateral); + const double lon = std::max(std::abs(longitudinal), ep); + const double v = std::abs(velocity); + return 0.5 * lat * std::pow(4.0 * v / lon, 3); +} + +double PathShifter::getTotalShiftLength() const +{ + double sum = base_offset_; + for (const auto & l : shift_lines_) { + sum += l.end_shift_length; + } + return sum; +} + +double PathShifter::getLastShiftLength() const +{ + if (shift_lines_.empty()) { + return base_offset_; + } + + const auto furthest = std::max_element( + shift_lines_.begin(), shift_lines_.end(), + [](auto & a, auto & b) { return a.end_idx < b.end_idx; }); + + return furthest->end_shift_length; +} + +boost::optional PathShifter::getLastShiftLine() const +{ + if (shift_lines_.empty()) { + return {}; + } + + const auto furthest = std::max_element( + shift_lines_.begin(), shift_lines_.end(), + [](auto & a, auto & b) { return a.end_idx > b.end_idx; }); + + return *furthest; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index eda5b740abdd9..5a590f9ae473f 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -433,9 +433,9 @@ void TurnSignalDecider::set_intersection_info( } intersection_distance_ = dist_to_intersection_required_start; intersection_pose_point_ = inter_required_start_point; + } else { + initialize_intersection_info(); } - - initialize_intersection_info(); } void TurnSignalDecider::initialize_intersection_info() diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 056632edf0d31..f4842edf7edd5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -76,14 +76,15 @@ size_t findNearestSegmentIndex( template size_t findNearestSegmentIndexFromLateralDistance( - const std::vector & points, const geometry_msgs::msg::Pose & pose) + const std::vector & points, const geometry_msgs::msg::Point & target_point) { - size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); + size_t closest_idx = motion_utils::findNearestSegmentIndex(points, target_point); double min_lateral_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, closest_idx, pose.position); + std::fabs(motion_utils::calcLateralOffset(points, target_point, closest_idx)); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); const double segment_length = tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); if (lon_dist < 0.0 || segment_length < lon_dist) { @@ -91,14 +92,24 @@ size_t findNearestSegmentIndexFromLateralDistance( } const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); + std::fabs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; + min_lateral_dist = lat_dist; } } return closest_idx; } + +bool checkHasSameLane( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelet & target_lane) +{ + if (lanelets.empty()) return false; + + const auto has_same = [&](const auto & ll) { return ll.id() == target_lane.id(); }; + return std::find_if(lanelets.begin(), lanelets.end(), has_same) != lanelets.end(); +} } // namespace namespace behavior_path_planner::util @@ -541,27 +552,30 @@ std::vector filterObjectIndicesByLanelets( return indices; } -// works with random lanelets -std::vector filterObjectIndicesByLanelets( +std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { - std::vector indices; if (target_lanelets.empty()) { return {}; } + std::vector target_indices; + std::vector other_indices; + for (size_t i = 0; i < objects.objects.size(); i++) { // create object polygon const auto & obj = objects.objects.at(i); // create object polygon Polygon2d obj_polygon; - if (!calcObjectPolygon(obj, &obj_polygon)) { + if (!util::calcObjectPolygon(obj, &obj_polygon)) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("utilities"), "Failed to calcObjectPolygon...!!!"); continue; } + bool is_filtered_object = false; + for (const auto & llt : target_lanelets) { // create lanelet polygon const auto polygon2d = llt.polygon2d().basicPolygon(); @@ -570,33 +584,47 @@ std::vector filterObjectIndicesByLanelets( continue; } Polygon2d lanelet_polygon; - lanelet_polygon.outer().reserve(polygon2d.size() + 1); for (const auto & lanelet_point : polygon2d) { lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - indices.push_back(i); + target_indices.push_back(i); + is_filtered_object = true; break; } } + + if (!is_filtered_object) { + other_indices.push_back(i); + } } - return indices; + + return std::make_pair(target_indices, other_indices); } -PredictedObjects filterObjectsByLanelets( +std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { - PredictedObjects filtered_objects; - const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets); - filtered_objects.objects.reserve(indices.size()); - for (const size_t i : indices) { - filtered_objects.objects.push_back(objects.objects.at(i)); + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); } - return filtered_objects; + + return std::make_pair(target_objects, other_objects); } bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon) @@ -1092,36 +1120,36 @@ std::vector cutOverlappedLanes( return shorten_lanes; } -geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( - const std::vector & points, const geometry_msgs::msg::Pose & pose, +geometry_msgs::msg::Point calcLongitudinalOffsetStartPoint( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { const double offset_length = motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_pose = - motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + const auto offset_point = + motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx); + return offset_point ? offset_point.get() : points.at(nearest_segment_idx); } -geometry_msgs::msg::Pose calcLongitudinalOffsetGoalPose( - const std::vector & points, const geometry_msgs::msg::Pose & pose, +geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { const double offset_length = motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_pose = - motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + const auto offset_point = + motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx + 1); + return offset_point ? offset_point.get() : points.at(nearest_segment_idx + 1); } void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, - const std::shared_ptr planner_data) + const std::shared_ptr planner_data, const bool is_driving_forward) { - std::vector left_bound; - std::vector right_bound; + std::vector left_bound; + std::vector right_bound; // extract data const auto transformed_lanes = util::transformToLanelets(lanes); @@ -1129,121 +1157,118 @@ void generateDrivableArea( const auto route_handler = planner_data->route_handler; constexpr double overlap_threshold = 0.01; - auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { - for (const auto & lp : lane.leftBound()) { - geometry_msgs::msg::Pose current_pose; - current_pose.position = lanelet::utils::conversion::toGeomMsgPt(lp); - if (left_bound.empty()) { - left_bound.push_back(current_pose); - } else if ( - tier4_autoware_utils::calcDistance2d(current_pose, left_bound.back()) > overlap_threshold) { - left_bound.push_back(current_pose); + auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } } - } - }; + }; - auto addRightBoundPoints = [&right_bound](const lanelet::ConstLanelet & lane) { - for (const auto & rp : lane.rightBound()) { - geometry_msgs::msg::Pose current_pose; - current_pose.position = lanelet::utils::conversion::toGeomMsgPt(rp); - if (right_bound.empty()) { - right_bound.push_back(current_pose); - } else if ( - tier4_autoware_utils::calcDistance2d(current_pose, right_bound.back()) > - overlap_threshold) { - right_bound.push_back(current_pose); + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::Id & ignore_lane_id = lanelet::InvalId) { + for (const auto & transformed_lane : transformed_lanes) { + if (transformed_lane.id() == ignore_lane_id) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } } - } - }; + return false; + }; // Insert Position for (const auto & lane : lanes) { - addLeftBoundPoints(lane.left_lane); - addRightBoundPoints(lane.right_lane); + addPoints(lane.left_lane.leftBound3d(), left_bound); + addPoints(lane.right_lane.rightBound3d(), right_bound); } - const auto has_same_lane = [&](const auto & lane) { - if (lanes.empty()) return false; - const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; - return std::find_if(transformed_lanes.begin(), transformed_lanes.end(), has_same) != - transformed_lanes.end(); - }; - - const auto has_overlap = [&](const auto & lane) { - for (const auto & transformed_lane : transformed_lanes) { - if (boost::geometry::intersects( - lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; - // Insert points after goal - if (containsGoal(transformed_lanes, route_handler->getGoalLaneId())) { + lanelet::ConstLanelet goal_lanelet; + if ( + route_handler->getGoalLanelet(&goal_lanelet) && + checkHasSameLane(transformed_lanes, goal_lanelet)) { const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); for (const auto & lane : lanes_after_goal) { - if (has_same_lane(lane) || has_overlap(lane)) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(transformed_lanes, lane)) { + continue; + } + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) + ? has_overlap(lane, route_handler->getGoalLaneId()) + : has_overlap(lane)); + if (is_overlapped) { continue; } - addLeftBoundPoints(lane); - addRightBoundPoints(lane); + + addPoints(lane.leftBound3d(), left_bound); + addPoints(lane.rightBound3d(), right_bound); } } - // Give Orientation - motion_utils::insertOrientation(left_bound, true); - motion_utils::insertOrientation(right_bound, true); + if (!is_driving_forward) { + std::reverse(left_bound.begin(), left_bound.end()); + std::reverse(right_bound.begin(), right_bound.end()); + } // Get Closest segment for the start point constexpr double front_length = 0.5; const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose); - const auto left_start_pose = - calcLongitudinalOffsetStartPose(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_pose = - calcLongitudinalOffsetStartPose(right_bound, front_pose, front_right_start_idx, -front_length); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + const auto left_start_point = + calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); + const auto right_start_point = + calcLongitudinalOffsetStartPoint(right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_pose); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose); - const auto left_goal_pose = - calcLongitudinalOffsetGoalPose(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_pose = - calcLongitudinalOffsetGoalPose(right_bound, goal_pose, goal_right_start_idx, vehicle_length); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); + const auto left_goal_point = + calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); + const auto right_goal_point = + calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point); const size_t right_goal_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_pose); + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point); // Store Data path.left_bound.clear(); path.right_bound.clear(); // Insert a start point - path.left_bound.push_back(left_start_pose.position); - path.right_bound.push_back(right_start_pose.position); + path.left_bound.push_back(left_start_point); + path.right_bound.push_back(right_start_point); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i).position; + const auto & next_point = left_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); if (dist > overlap_threshold) { path.left_bound.push_back(next_point); } } for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i).position; + const auto & next_point = right_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); if (dist > overlap_threshold) { path.right_bound.push_back(next_point); @@ -1252,14 +1277,14 @@ void generateDrivableArea( // Insert a goal point if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_pose.position) > + tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > overlap_threshold) { - path.left_bound.push_back(left_goal_pose.position); + path.left_bound.push_back(left_goal_point); } if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_pose.position) > + tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > overlap_threshold) { - path.right_bound.push_back(right_goal_pose.position); + path.right_bound.push_back(right_goal_point); } } @@ -1518,6 +1543,61 @@ double getSignedDistanceFromShoulderLeftBoundary( return arc_coordinates.distance; } +std::optional getSignedDistanceFromShoulderLeftBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose) +{ + double min_distance = std::numeric_limits::max(); + const auto transformed_footprint = + transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); + bool found_neighbor_shoulder_bound = false; + for (const auto & vehicle_corner_point : transformed_footprint) { + // convert point of footprint to pose + Pose vehicle_corner_pose{}; + vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.orientation = vehicle_pose.orientation; + + // calculate distance to the shoulder bound directly next to footprint points + lanelet::ConstLanelet closest_shoulder_lanelet{}; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets, vehicle_corner_pose, &closest_shoulder_lanelet)) { + const auto & left_line_2d = lanelet::utils::to2D(closest_shoulder_lanelet.leftBound3d()); + + for (size_t i = 1; i < left_line_2d.size(); ++i) { + const Point p_front = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i - 1]); + const Point p_back = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i]); + + const Point inverse_p_front = + tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); + const Point inverse_p_back = + tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); + + const double dy_front = inverse_p_front.y; + const double dy_back = inverse_p_back.y; + const double dx_front = inverse_p_front.x; + const double dx_back = inverse_p_back.x; + // is in segment + if (dx_front < 0 && dx_back > 0) { + const double lateral_distance_from_pose_to_segment = + (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); + min_distance = std::min(lateral_distance_from_pose_to_segment, min_distance); + found_neighbor_shoulder_bound = true; + break; + } + } + } + } + + if (!found_neighbor_shoulder_bound) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utilities"), + "neighbor shoulder bound to footprint is not found."); + return {}; + } + + return -min_distance; +} + double getSignedDistanceFromRightBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose) { @@ -1657,7 +1737,7 @@ std::shared_ptr generateCenterLinePath( // TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if // we can refactor some of the code for better readability -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data) { const auto & p = planner_data->parameters; @@ -2382,7 +2462,7 @@ bool isSafeInLaneletCollisionCheck( const double check_start_time, const double check_end_time, const double check_time_resolution, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug) + const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug) { const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution; if (lerp_path_reserve > 1e-3) { @@ -2422,6 +2502,7 @@ bool isSafeInLaneletCollisionCheck( debug.failed_reason = "not_enough_longitudinal"; return false; } + ego_pose_before_collision = expected_ego_pose; } return true; } @@ -2515,19 +2596,19 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } -double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param) +double calcTotalLaneChangeDistance( + const BehaviorPathPlannerParameters & common_param, const bool include_buffer) { const double minimum_lane_change_distance = common_param.minimum_lane_change_prepare_distance + common_param.minimum_lane_change_length; const double end_of_lane_buffer = common_param.backward_length_buffer_for_end_of_lane; - return minimum_lane_change_distance + end_of_lane_buffer; + return minimum_lane_change_distance + end_of_lane_buffer * static_cast(include_buffer); } double calcLaneChangeBuffer( const BehaviorPathPlannerParameters & common_param, const int num_lane_change, const double length_to_intersection) { - return num_lane_change * calcTotalLaneChangeDistanceWithBuffer(common_param) + - length_to_intersection; + return num_lane_change * calcTotalLaneChangeDistance(common_param) + length_to_intersection; } } // namespace behavior_path_planner::util diff --git a/planning/behavior_velocity_planner/config/blind_spot.param.yaml b/planning/behavior_velocity_planner/config/blind_spot.param.yaml index 31f75d7f7c5df..5af1c99f8a0c8 100644 --- a/planning/behavior_velocity_planner/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/blind_spot.param.yaml @@ -6,3 +6,4 @@ backward_length: 15.0 # [m] ignore_width_from_center_line: 0.7 # [m] max_future_movement_time: 10.0 # [second] + threshold_yaw_diff: 0.523 # [rad] diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -6,7 +6,7 @@ keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp index 40155ae90f4fb..371c0cedfb174 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -40,6 +40,7 @@ namespace behavior_velocity_planner { +using autoware_auto_mapping_msgs::msg::HADMapBin; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node { @@ -109,6 +110,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node PlannerData planner_data_; BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + bool has_received_map_; // mutex for planner_data_ std::mutex mutex_; diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index e3cacf3a362ba..4b1c672ff2b08 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -65,6 +65,7 @@ class BlindSpotModule : public SceneModuleInterface double ignore_width_from_center_line; //! ignore width from center line from detection_area double max_future_movement_time; //! maximum time[second] for considering future movement of object + double threshold_yaw_diff; //! threshold of yaw difference between ego and target object }; BlindSpotModule( @@ -153,7 +154,7 @@ class BlindSpotModule : public SceneModuleInterface */ bool isPredictedPathInArea( const autoware_auto_perception_msgs::msg::PredictedObject & object, - const lanelet::CompoundPolygon3d & area) const; + const lanelet::CompoundPolygon3d & area, geometry_msgs::msg::Pose ego_pose) const; /** * @brief Generate a stop line and insert it into the path. diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 8decae844adb7..c1097f39bbf01 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -166,7 +166,7 @@ class IntersectionModule : public SceneModuleInterface Polygon2d generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const; + const double extra_dist, const double ignore_dist) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index bfe03cced0a77..47ea7b5532278 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -183,6 +183,7 @@ struct PossibleCollisionInfo struct DebugData { double z; + double baselink_to_front; std::string road_type = ""; std::string detection_type = ""; Polygons2d detection_area_polygons; @@ -190,8 +191,10 @@ struct DebugData std::vector parked_vehicle_point; std::vector possible_collisions; std::vector occlusion_points; + std::vector debug_poses; void resetData() { + debug_poses.clear(); close_partition.clear(); detection_area_polygons.clear(); parked_vehicle_point.clear(); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp index c201cc4158907..15eac76e35c48 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -28,7 +28,7 @@ namespace occlusion_spot_utils { void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, - const PlannerParam & param); + std::vector & debug_poses, const PlannerParam & param); /** * @param: v: ego velocity config diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 3995866f760c6..b795d84f8f421 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -7,6 +7,7 @@ Mamoru Sobue + Takayuki Murooka Satoshi Ota diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 5fea6f0a13e82..ab5d31e01c224 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -238,12 +238,7 @@ bool BehaviorVelocityPlannerNode::isDataReady( RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud"); return false; } - if (!d.route_handler_) { - RCLCPP_INFO_THROTTLE( - get_logger(), clock, 3000, "Waiting for the initialization of route_handler"); - return false; - } - if (!d.route_handler_->isMapMsgReady()) { + if (!map_ptr_) { RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map"); return false; } @@ -344,8 +339,8 @@ void BehaviorVelocityPlannerNode::onLaneletMap( { std::lock_guard lock(mutex_); - // Load map - planner_data_.route_handler_ = std::make_shared(*msg); + map_ptr_ = msg; + has_received_map_ = true; } void BehaviorVelocityPlannerNode::onTrafficSignals( @@ -419,6 +414,18 @@ void BehaviorVelocityPlannerNode::onTrigger( return; } + // Load map and check route handler + if (has_received_map_) { + planner_data_.route_handler_ = std::make_shared(*map_ptr_); + has_received_map_ = false; + } + if (!planner_data_.route_handler_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, "Waiting for the initialization of route_handler"); + mutex_.unlock(); + return; + } + // NOTE: planner_data must not be referenced for multithreading const auto planner_data = planner_data_; mutex_.unlock(); diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index df5223d12f7d9..2d0ecee9cee30 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -36,6 +36,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".ignore_width_from_center_line", 1.0); planner_param_.max_future_movement_time = node.declare_parameter(ns + ".max_future_movement_time", 10.0); + planner_param_.threshold_yaw_diff = + node.declare_parameter(ns + ".threshold_yaw_diff", M_PI / 6.0); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 1b64566611cfe..4b81d11e4bcbb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -389,7 +389,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( bool exist_in_detection_area = bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(areas_opt.get().detection_area)); - bool exist_in_conflict_area = isPredictedPathInArea(object, areas_opt.get().conflict_area); + bool exist_in_conflict_area = isPredictedPathInArea( + object, areas_opt.get().conflict_area, planner_data_->current_pose.pose); if (exist_in_detection_area || exist_in_conflict_area) { obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); @@ -403,16 +404,23 @@ bool BlindSpotModule::checkObstacleInBlindSpot( bool BlindSpotModule::isPredictedPathInArea( const autoware_auto_perception_msgs::msg::PredictedObject & object, - const lanelet::CompoundPolygon3d & area) const + const lanelet::CompoundPolygon3d & area, geometry_msgs::msg::Pose ego_pose) const { const auto area_2d = lanelet::utils::to2D(area); + const auto ego_yaw = tf2::getYaw(ego_pose.orientation); + const auto threshold_yaw_diff = planner_param_.threshold_yaw_diff; // NOTE: iterating all paths including those of low confidence return std::any_of( object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(), - [&area_2d](const auto & path) { - return std::any_of(path.path.begin(), path.path.end(), [&area_2d](const auto & point) { - return bg::within(to_bg2d(point.position), area_2d); - }); + [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & path) { + return std::any_of( + path.path.begin(), path.path.end(), + [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & point) { + const auto is_in_area = bg::within(to_bg2d(point.position), area_2d); + const auto match_yaw = + std::fabs(ego_yaw - tf2::getYaw(point.orientation)) < threshold_yaw_diff; + return is_in_area && match_yaw; + }); }); } diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 9b40751449dd6..6b71d8a5cf4a2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -39,17 +39,10 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; +using tier4_autoware_utils::toHexString; namespace { -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) { geometry_msgs::msg::Point32 p; @@ -453,7 +446,7 @@ boost::optional CrosswalkModule::findNearestStopPoint const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance; const auto margin = stop_at_stop_line ? stop_line_distance + base_link2front : planner_param_.stop_margin + base_link2front; - const auto stop_pose = calcLongitudinalOffsetPose(sparse_resample_path.points, p_stop, -margin); + const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop, -margin); if (!stop_pose) { return {}; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d3c20427f118e..0b617373ff5ba 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -151,26 +151,22 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const size_t closest_idx = closest_idx_opt.get(); if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines.stop_line; - const size_t pass_judge_line_idx = stop_lines.pass_judge_line; - const size_t keep_detection_line_idx = stop_lines.keep_detection_line; + const auto stop_line_idx = stop_lines_idx_opt.value().stop_line; + const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; + const auto keep_detection_line_idx = stop_lines_idx_opt.value().keep_detection_line; const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); const bool is_before_keep_detection_line = - util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); + stop_lines_idx_opt.has_value() + ? util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx) + : false; const bool keep_detection = is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; if (is_over_pass_judge_line && keep_detection) { // in case ego could not stop exactly before the stop line, but with some overshoot, // keep detection within some margin under low velocity threshold - RCLCPP_DEBUG( - logger_, - "over the pass judge line, but before keep detection line and low speed, " - "continue planning"); - // no return here, keep planning } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); @@ -178,71 +174,73 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); - // no plan needed. return true; } } + /* collision checking */ + bool is_entry_prohibited = false; /* get dynamic object */ const auto objects_ptr = planner_data_->predicted_objects; - /* calculate final stop lines */ - bool is_entry_prohibited = false; - const double detect_length = + /* check stuck vehicle */ + const double ignore_length = + planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; + const double detect_dist = planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( - lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length, - planner_param_.stuck_vehicle_detect_dist); + lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); - int stop_line_idx_final = stuck_line_idx; - int pass_judge_line_idx_final = stuck_line_idx; + + /* calculate dynamic collision around detection area */ + const bool has_collision = checkCollision( + lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, + closest_idx, stuck_vehicle_detect_area); + + /* calculate final stop lines */ + int stop_line_idx_final = + stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1; + int pass_judge_line_idx_final = + stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().pass_judge_line : -1; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck) { + } else if (is_stuck || has_collision) { is_entry_prohibited = true; - stop_line_idx_final = stuck_line_idx; - pass_judge_line_idx_final = stuck_line_idx; - } else { - /* calculate dynamic collision around detection area */ - const bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, - closest_idx, stuck_vehicle_detect_area); - is_entry_prohibited = has_collision; - if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - stop_line_idx_final = stop_lines_idx.stop_line; - pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; - } else { - if (has_collision) { - RCLCPP_ERROR(logger_, "generateStopLine() failed for detected objects"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return false; - } else { - RCLCPP_DEBUG(logger_, "no need to stop"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; - } + const double dist_stuck_stopline = motion_utils::calcSignedArcLength( + path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points.at(closest_idx).point.pose.position); + const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline + const bool is_over_stuck_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) && + dist_stuck_stopline > eps; + if (is_stuck && !is_over_stuck_stopline) { + stop_line_idx_final = stuck_line_idx; + pass_judge_line_idx_final = stuck_line_idx; + } else if ( + ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { + stop_line_idx_final = stop_lines_idx_opt.value().stop_line; + pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line; } } + if (stop_line_idx_final == -1) { + RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + state_machine_.setStateWithMarginTime( is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); setSafe(state_machine_.getState() == StateMachine::State::GO); - if (is_entry_prohibited) { - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); - } else { - setDistance(std::numeric_limits::lowest()); - } + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); if (!isActivated()) { // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block @@ -318,8 +316,11 @@ bool IntersectionModule::checkCollision( using lanelet::utils::getPolygonFromArcLength; /* generate ego-lane polygon */ - const auto ego_poly = - generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); + const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d(); + Polygon2d ego_poly{}; + for (const auto & p : ego_lane_poly) { + ego_poly.outer().emplace_back(p.x(), p.y()); + } lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); lanelet::ConstLanelet closest_lanelet; lanelet::utils::query::getClosestLanelet( @@ -479,7 +480,7 @@ bool IntersectionModule::checkCollision( Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const + const double extra_dist, const double ignore_dist) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -488,15 +489,14 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); - const auto start_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point)); + const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); const auto closest_arc_coords = getArcCoordinates( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length - ? closest_arc_coords.length - : start_arc_coords.length + ignore_dist; + const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length + ? intersection_exit_length - ignore_dist + : closest_arc_coords.length; const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index a7e1739c38119..1f4cba618fe28 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -148,7 +148,7 @@ std::pair, std::optional> generateStopLine( const double keep_detection_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -196,13 +196,6 @@ std::pair, std::optional> generateStopLine( if (use_stuck_stopline) { // the first point in intersection lane stuck_stop_line_idx_ip = lane_interval_ip_start; - if (stuck_stop_line_idx_ip == 0) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, *clock, 1000 /* ms */, - "use_stuck_stopline, but ego is already in the intersection, not generating stuck stop " - "line"); - return {std::nullopt, std::nullopt}; - } } else { const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 76e71b67a2d03..73c8b12b79162 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -219,13 +219,12 @@ MarkerArray OcclusionSpotModule::createVirtualWallMarkerArray() MarkerArray wall_marker; std::string module_name = "occlusion_spot"; - if (!debug_data_.possible_collisions.empty()) { - for (size_t id = 0; id < debug_data_.possible_collisions.size(); id++) { - const auto & pose = debug_data_.possible_collisions.at(id).intersection_pose; - appendMarkerArray( - motion_utils::createSlowDownVirtualWallMarker(pose, module_name, current_time, id), - &wall_marker, current_time); - } + for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { + const auto p_front = + calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); + appendMarkerArray( + motion_utils::createSlowDownVirtualWallMarker(p_front, module_name, current_time, id), + &wall_marker, current_time); } return wall_marker; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index ec20be3a2457c..d8507cb5a77c8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -283,16 +283,13 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const ArcCoordinates & arc_coord_occlusion_with_offset, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { - const auto & ll = pl.centerline2d(); + auto calcPosition = [](const ConstLineString2d & ll, const ArcCoordinates & arc) { BasicPoint2d bp = fromArcCoordinates(ll, arc); - Pose pose; - pose.position.x = bp[0]; - pose.position.y = bp[1]; - pose.position.z = 0.0; - pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - lanelet::utils::getLaneletAngle(pl, pose.position)); - return pose; + Point position; + position.x = bp[0]; + position.y = bp[1]; + position.z = 0.0; + return position; }; /** * @brief calculate obstacle collision intersection from arc coordinate info. @@ -316,11 +313,13 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( pc.arc_lane_dist_at_collision = {distance_to_stop, arc_coord_occlusion_with_offset.distance}; pc.obstacle_info.safe_motion = sm; pc.obstacle_info.ttv = ttv; - pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position; + + const auto & ll = path_lanelet.centerline2d(); + pc.obstacle_info.position = calcPosition(ll, arc_coord_occlusion); pc.obstacle_info.max_velocity = param.pedestrian_vel; - pc.collision_pose = calcPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); - pc.collision_with_margin.pose = calcPose(path_lanelet, {distance_to_stop, 0.0}); - pc.intersection_pose = calcPose(path_lanelet, {arc_coord_occlusion.length, 0.0}); + pc.collision_pose.position = calcPosition(ll, {arc_coord_occlusion_with_offset.length, 0.0}); + pc.collision_with_margin.pose.position = calcPosition(ll, {distance_to_stop, 0.0}); + pc.intersection_pose.position = calcPosition(ll, {arc_coord_occlusion.length, 0.0}); return pc; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 3e7ff2c9e281a..f4817428af445 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -25,7 +25,7 @@ namespace occlusion_spot_utils { void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, - const PlannerParam & param) + std::vector & debug_poses, const PlannerParam & param) { // return nullptr or too few points if (!inout_path || inout_path->points.size() < 2) { @@ -60,7 +60,9 @@ void applySafeVelocityConsideringPossibleCollision( safe_velocity = std::max(safe_velocity, v_min); possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity; const auto & pose = possible_collision.collision_with_margin.pose; - planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); + const auto & decel_pose = + planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); + if (decel_pose) debug_poses.push_back(decel_pose.get()); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 9bafd9e707741..1d09cac0bceeb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -183,7 +183,9 @@ bool OcclusionSpotModule::modifyPathVelocity( // Note: Consider offset from path start to ego here utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration - utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); + utils::applySafeVelocityConsideringPossibleCollision( + path, possible_collisions, debug_data_.debug_poses, param_); + debug_data_.baselink_to_front = param_.baselink_to_front; // these debug topics needs computation resource debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; diff --git a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 0786e592fe363..93fad135ac678 100644 --- a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -17,6 +17,7 @@ #include +#include #include #include @@ -25,10 +26,13 @@ #include #include +using tier4_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using tier4_planning_msgs::msg::VelocityLimitConstraints; +using VelocityLimitTable = std::unordered_map; + class ExternalVelocityLimitSelectorNode : public rclcpp::Node { public: @@ -58,18 +62,20 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_external_velocity_limit_from_internal_; rclcpp::Subscription::SharedPtr sub_velocity_limit_clear_command_; rclcpp::Publisher::SharedPtr pub_external_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_debug_string_; void publishVelocityLimit(const VelocityLimit & velocity_limit); void setVelocityLimitFromAPI(const VelocityLimit & velocity_limit); void setVelocityLimitFromInternal(const VelocityLimit & velocity_limit); void clearVelocityLimit(const std::string & sender); void updateVelocityLimit(); + void publishDebugString(); VelocityLimit getCurrentVelocityLimit() { return hardest_limit_; } // Parameters NodeParam node_param_{}; VelocityLimit hardest_limit_{}; - std::unordered_map velocity_limit_table_; + VelocityLimitTable velocity_limit_table_; }; #endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index 9cc9141b09807..5ef089f3d3ee7 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -8,6 +8,7 @@ + @@ -16,5 +17,6 @@ + diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/external_velocity_limit_selector/package.xml index 2a0d2ff51680c..59456adac97ae 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/external_velocity_limit_selector/package.xml @@ -5,6 +5,11 @@ 0.1.0 The external_velocity_limit_selector ROS2 package Satoshi Ota + Shinnosuke Hirakawa + Shumpei Wakabayashi + Tomohito Ando + Tomoya Kimura + Apache License 2.0 Satoshi Ota @@ -15,6 +20,7 @@ rclcpp rclcpp_components + tier4_debug_msgs tier4_planning_msgs ros2cli diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index b4c3d0f5e7758..bda1370545edc 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -24,7 +24,7 @@ namespace { VelocityLimit getHardestLimit( - const std::unordered_map & velocity_limits, + const VelocityLimitTable & velocity_limits, const ExternalVelocityLimitSelectorNode::NodeParam & node_param) { VelocityLimit hardest_limit{}; @@ -65,6 +65,23 @@ VelocityLimit getHardestLimit( return hardest_limit; } + +std::string getDebugString(const VelocityLimitTable & velocity_limits) +{ + std::ostringstream string_stream; + string_stream << std::boolalpha << std::fixed << std::setprecision(2); + for (const auto & limit : velocity_limits) { + string_stream << "[" << limit.first << "]"; + string_stream << "("; + string_stream << limit.second.use_constraints << ","; + string_stream << limit.second.max_velocity << ","; + string_stream << limit.second.constraints.min_acceleration << ","; + string_stream << limit.second.constraints.min_jerk << ","; + string_stream << limit.second.constraints.max_jerk << ")"; + } + + return string_stream.str(); +} } // namespace ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode( @@ -78,17 +95,19 @@ ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode( std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitFromAPI, this, _1)); sub_external_velocity_limit_from_internal_ = this->create_subscription( - "input/velocity_limit_from_internal", rclcpp::QoS{1}.transient_local(), + "input/velocity_limit_from_internal", rclcpp::QoS{10}.transient_local(), std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal, this, _1)); sub_velocity_limit_clear_command_ = this->create_subscription( - "input/velocity_limit_clear_command_from_internal", rclcpp::QoS{1}.transient_local(), + "input/velocity_limit_clear_command_from_internal", rclcpp::QoS{10}.transient_local(), std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand, this, _1)); // Output pub_external_velocity_limit_ = this->create_publisher("output/external_velocity_limit", 1); + pub_debug_string_ = this->create_publisher("output/debug", 1); + // Params { auto & p = node_param_; @@ -112,6 +131,8 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitFromAPI( const auto velocity_limit = getCurrentVelocityLimit(); publishVelocityLimit(velocity_limit); + + publishDebugString(); } void ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal( @@ -122,6 +143,8 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal( const auto velocity_limit = getCurrentVelocityLimit(); publishVelocityLimit(velocity_limit); + + publishDebugString(); } void ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand( @@ -135,6 +158,8 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand( const auto velocity_limit = getCurrentVelocityLimit(); publishVelocityLimit(velocity_limit); + + publishDebugString(); } void ExternalVelocityLimitSelectorNode::publishVelocityLimit(const VelocityLimit & velocity_limit) @@ -142,6 +167,14 @@ void ExternalVelocityLimitSelectorNode::publishVelocityLimit(const VelocityLimit pub_external_velocity_limit_->publish(velocity_limit); } +void ExternalVelocityLimitSelectorNode::publishDebugString() +{ + StringStamped debug_string{}; + debug_string.stamp = this->now(); + debug_string.data = getDebugString(velocity_limit_table_); + pub_debug_string_->publish(debug_string); +} + void ExternalVelocityLimitSelectorNode::setVelocityLimitFromAPI( const VelocityLimit & velocity_limit) { diff --git a/planning/freespace_planning_algorithms/README.md b/planning/freespace_planning_algorithms/README.md index 72b1c9fc985eb..ef0c2a3aca384 100644 --- a/planning/freespace_planning_algorithms/README.md +++ b/planning/freespace_planning_algorithms/README.md @@ -1,4 +1,4 @@ -# `freespace planning algorithms` +# freespace planning algorithms ## Role @@ -48,7 +48,7 @@ colcon build --packages-select freespace_planning_algorithms colcon test --packages-select freespace_planning_algorithms ``` -Inside the test, simulation results are stored in `/tmp/fpalgos-{#algorithm_type}-case{#scenario_number}` as a rosbag. +Inside the test, simulation results are stored in `/tmp/fpalgos-{algorithm_type}-case{scenario_number}` as a rosbag. Loading these resulting files, by using [test/debug_plot.py](test/debug_plot.py), one can create plots visualizing the path and obstacles as shown in the figures below. The created figures are then again saved in `/tmp`. diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index 17171882f0bda..b59888fa52281 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -114,7 +114,9 @@ class AbstractPlanningAlgorithm virtual ~AbstractPlanningAlgorithm() {} protected: - void computeCollisionIndexes(int theta_index, std::vector & indexes) const; + void computeCollisionIndexes( + int theta_index, std::vector & indexes, + std::vector & vertex_indexes_2d) const; bool detectCollision(const IndexXYT & base_index) const; inline bool isOutOfRange(const IndexXYT & index) const { @@ -143,6 +145,9 @@ class AbstractPlanningAlgorithm // collision indexes cache std::vector> coll_indexes_table_; + // vehicle vertex indexes cache + std::vector> vertex_indexes_table_; + // is_obstacle's table std::vector> is_obstacle_table_; diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 2f2328c1a43da..9ccec85bfc185 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include namespace freespace_planning_algorithms @@ -130,6 +131,11 @@ class AstarSearch : public AbstractPlanningAlgorithm const PlannerWaypoints & getWaypoints() const { return waypoints_; } + inline int getKey(const IndexXYT & index) + { + return (index.theta + (index.y * x_scale_ + index.x) * y_scale_); + } + private: bool search(); void clearNodes(); @@ -139,14 +145,18 @@ class AstarSearch : public AbstractPlanningAlgorithm double estimateCost(const geometry_msgs::msg::Pose & pose) const; bool isGoal(const AstarNode & node) const; - AstarNode * getNodeRef(const IndexXYT & index) { return &nodes_[index.y][index.x][index.theta]; } + AstarNode * getNodeRef(const IndexXYT & index) + { + return &(graph_.emplace(getKey(index), AstarNode()).first->second); + } // Algorithm specific param AstarParam astar_param_; // hybrid astar variables TransitionTable transition_table_; - std::vector>> nodes_; + std::unordered_map graph_; + std::priority_queue, NodeComparison> openlist_; // goal node, which may helpful in testing and debugging @@ -154,6 +164,9 @@ class AstarSearch : public AbstractPlanningAlgorithm // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; + + int x_scale_; + int y_scale_; }; } // namespace freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index b4ab7e62750d9..143f147b79eee 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -121,14 +121,16 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost // construct collision indexes table for (int i = 0; i < planner_common_param_.theta_size; i++) { - std::vector indexes_2d; - computeCollisionIndexes(i, indexes_2d); + std::vector indexes_2d, vertex_indexes_2d; + computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d); coll_indexes_table_.push_back(indexes_2d); + vertex_indexes_table_.push_back(vertex_indexes_2d); } } void AbstractPlanningAlgorithm::computeCollisionIndexes( - int theta_index, std::vector & indexes_2d) const + int theta_index, std::vector & indexes_2d, + std::vector & vertex_indexes_2d) const { IndexXYT base_index{0, 0, theta_index}; const VehicleShape & vehicle_shape = collision_vehicle_shape_; @@ -143,7 +145,8 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( const auto base_theta = tf2::getYaw(base_pose.orientation); // Convert each point to index and check if the node is Obstacle - const auto addIndex2d = [&](const double x, const double y) { + const auto addIndex2d = [&]( + const double x, const double y, std::vector & indexes_cache) { // Calculate offset in rotated frame const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; @@ -154,19 +157,41 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( const auto index = pose2index(costmap_, pose_local, planner_common_param_.theta_size); const auto index_2d = IndexXY{index.x, index.y}; - indexes_2d.push_back(index_2d); + indexes_cache.push_back(index_2d); }; for (double x = back; x <= front; x += costmap_.info.resolution / 2) { for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(x, y); + addIndex2d(x, y, indexes_2d); } - addIndex2d(x, left); + addIndex2d(x, left, indexes_2d); } for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(front, y); + addIndex2d(front, y, indexes_2d); } - addIndex2d(front, left); + addIndex2d(front, left, indexes_2d); + + const auto compareIndex2d = [](const IndexXY & left, const IndexXY & right) { + if (left.x != right.x) { + return (left.x < right.x); + } else { + return (left.y < right.y); + } + }; + + const auto equalIndex2d = [](const IndexXY & left, const IndexXY & right) { + return ((left.x == right.x) && (left.y == right.y)); + }; + + // remove duplicate indexes + std::sort(indexes_2d.begin(), indexes_2d.end(), compareIndex2d); + indexes_2d.erase( + std::unique(indexes_2d.begin(), indexes_2d.end(), equalIndex2d), indexes_2d.end()); + + addIndex2d(front, left, vertex_indexes_2d); + addIndex2d(front, right, vertex_indexes_2d); + addIndex2d(back, right, vertex_indexes_2d); + addIndex2d(back, left, vertex_indexes_2d); } bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const @@ -175,6 +200,18 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; return false; } + + const auto & vertex_indexes_2d = vertex_indexes_table_[base_index.theta]; + for (const auto & vertex_index_2d : vertex_indexes_2d) { + IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0}; + // must slide to current base position + vertex_index.x += base_index.x; + vertex_index.y += base_index.y; + if (isOutOfRange(vertex_index)) { + return true; + } + } + const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { int idx_theta = 0; // whatever. Yaw is nothing to do with collision detection between grids. @@ -183,10 +220,11 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con coll_index.x += base_index.x; coll_index.y += base_index.y; - if (isOutOfRange(coll_index) || isObs(coll_index)) { + if (isObs(coll_index)) { return true; } } + return false; } diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 315b9e06cf812..8d419c3daa13f 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -133,6 +133,8 @@ AstarSearch::AstarSearch( planner_common_param_.minimum_turning_radius, planner_common_param_.maximum_turning_radius, planner_common_param_.turning_radius_size, planner_common_param_.theta_size, astar_param_.use_back); + + y_scale_ = planner_common_param.theta_size; } void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) @@ -141,17 +143,8 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) clearNodes(); - const auto height = costmap_.info.height; - const auto width = costmap_.info.width; - - // Initialize nodes - nodes_.resize(height); - for (size_t i = 0; i < height; i++) { - nodes_[i].resize(width); - for (size_t j = 0; j < width; j++) { - nodes_[i][j].resize(planner_common_param_.theta_size); - } - } + x_scale_ = costmap_.info.height; + graph_.reserve(100000); } bool AstarSearch::makePlan( @@ -175,8 +168,9 @@ void AstarSearch::clearNodes() { // clearing openlist is necessary because otherwise remaining elements of openlist // point to deleted node. - nodes_.clear(); openlist_ = std::priority_queue, NodeComparison>(); + + graph_ = std::unordered_map(); } bool AstarSearch::setStartNode() diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 5000b73843a75..08c23c24f73d6 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -40,16 +40,12 @@ const double width_lexas = 2.75; const fpa::VehicleShape vehicle_shape = fpa::VehicleShape{length_lexas, width_lexas, 1.5}; const double pi = 3.1415926; const std::array start_pose{5.5, 4., pi * 0.5}; -const std::array goal_pose1{8.0, 26.3, pi * 1.5}; // easiest -const std::array, 1> goal_poses{goal_pose1}; - -// the tests for following goals randomly fail. needs to be fixed. -// https://github.com/autowarefoundation/autoware.universe/issues/2439 -// const std::array goal_pose2{15.0, 11.6, pi * 0.5}; // second easiest -// const std::array goal_pose3{18.4, 26.3, pi * 1.5}; // third easiest -// const std::array goal_pose4{25.0, 26.3, pi * 1.5}; // most difficult -// const std::array, 4> goal_poses{ -// goal_pose1, goal_pose2, goal_pose3, goal_pose4}; +const std::array goal_pose1{8.0, 26.3, pi * 1.5}; // easiest +const std::array goal_pose2{15.0, 11.6, pi * 0.5}; // second easiest +const std::array goal_pose3{18.4, 26.3, pi * 1.5}; // third easiest +const std::array goal_pose4{25.0, 26.3, pi * 1.5}; // most difficult +const std::array, 4> goal_poses{ + goal_pose1, goal_pose2, goal_pose3, goal_pose4}; geometry_msgs::msg::Pose create_pose_msg(std::array pose3d) { @@ -249,7 +245,7 @@ std::unordered_map rosbag_dir_prefix_table( {RRTSTAR_UPDATE, "fpalgos-rrtstar_update"}, {RRTSTAR_INFORMED_UPDATE, "fpalgos-rrtstar_informed_update"}}); -bool test_algorithm(enum AlgorithmType algo_type) +bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) { std::unique_ptr algo; if (algo_type == AlgorithmType::ASTAR_SINGLE) { @@ -272,8 +268,6 @@ bool test_algorithm(enum AlgorithmType algo_type) rclcpp::Clock clock{RCL_SYSTEM_TIME}; for (size_t i = 0; i < goal_poses.size(); ++i) { - const std::string dir_name = - "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i); const auto goal_pose = goal_poses.at(i); bool success_local = true; @@ -324,41 +318,46 @@ bool test_algorithm(enum AlgorithmType algo_type) success_all = false; } - rcpputils::fs::remove_all(dir_name); - - rosbag2_storage::StorageOptions storage_options; - storage_options.uri = dir_name; - storage_options.storage_id = "sqlite3"; - - rosbag2_cpp::ConverterOptions converter_options; - converter_options.input_serialization_format = "cdr"; - converter_options.output_serialization_format = "cdr"; - - rosbag2_cpp::Writer writer(std::make_unique()); - writer.open(storage_options, converter_options); - - add_message_to_rosbag( - writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64"); - add_message_to_rosbag( - writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64"); - add_message_to_rosbag( - writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back", - "std_msgs/msg/Float64"); - - add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid"); - add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose"); - add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose"); - add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray"); - add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64"); + if (dump_rosbag) { + // dump rosbag for visualization using python script + const std::string dir_name = + "/tmp/" + rosbag_dir_prefix_table[algo_type] + "-case" + std::to_string(i); + + rcpputils::fs::remove_all(dir_name); + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = dir_name; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + rosbag2_cpp::Writer writer(std::make_unique()); + writer.open(storage_options, converter_options); + + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.length), "vehicle_length", "std_msgs/msg/Float64"); + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.width), "vehicle_width", "std_msgs/msg/Float64"); + add_message_to_rosbag( + writer, create_float_msg(vehicle_shape.base2back), "vehicle_base2back", + "std_msgs/msg/Float64"); + + add_message_to_rosbag(writer, costmap_msg, "costmap", "nav_msgs/msg/OccupancyGrid"); + add_message_to_rosbag(writer, create_pose_msg(start_pose), "start", "geometry_msgs/msg/Pose"); + add_message_to_rosbag(writer, create_pose_msg(goal_pose), "goal", "geometry_msgs/msg/Pose"); + add_message_to_rosbag(writer, trajectory, "trajectory", "geometry_msgs/msg/PoseArray"); + add_message_to_rosbag(writer, create_float_msg(msec), "elapsed_time", "std_msgs/msg/Float64"); + } } return success_all; } -// the following test fails https://github.com/autowarefoundation/autoware.universe/issues/2439 -// TEST(AstarSearchTestSuite, SingleCurvature) -// { -// EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE)); -// } +TEST(AstarSearchTestSuite, SingleCurvature) +{ + EXPECT_TRUE(test_algorithm(AlgorithmType::ASTAR_SINGLE)); +} TEST(AstarSearchTestSuite, MultiCurvature) { diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index b741bd4479763..832df4a58fd1a 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -107,13 +107,10 @@ double project_goal_to_map( namespace mission_planner::lanelet2 { -void DefaultPlanner::initialize(rclcpp::Node * node) +void DefaultPlanner::initialize_common(rclcpp::Node * node) { is_graph_ready_ = false; node_ = node; - map_subscriber_ = node_->create_subscription( - "input/vector_map", rclcpp::QoS{10}.transient_local(), - std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_goal_footprint_marker_ = @@ -123,10 +120,17 @@ void DefaultPlanner::initialize(rclcpp::Node * node) param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg", 45.0); } +void DefaultPlanner::initialize(rclcpp::Node * node) +{ + initialize_common(node); + map_subscriber_ = node_->create_subscription( + "input/vector_map", rclcpp::QoS{10}.transient_local(), + std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); +} + void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) { - is_graph_ready_ = false; - node_ = node; + initialize_common(node); map_callback(msg); } @@ -272,6 +276,25 @@ bool DefaultPlanner::is_goal_valid( const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { const auto logger = node_->get_logger(); + + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); + + // check if goal is in shoulder lanelet + lanelet::Lanelet closest_shoulder_lanelet; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { + if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { + const auto lane_yaw = + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; + } + } + } + lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; @@ -298,8 +321,6 @@ bool DefaultPlanner::is_goal_valid( return false; } - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); - if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); @@ -323,23 +344,6 @@ bool DefaultPlanner::is_goal_valid( return true; } - // check if goal is in shoulder lanelet - lanelet::Lanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - return false; - } - // check if goal pose is in shoulder lane - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } - } return false; } diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 20f731953667a..4441856857f63 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -66,6 +66,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin rclcpp::Subscription::SharedPtr map_subscriber_; rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; + void initialize_common(rclcpp::Node * node); void map_callback(const HADMapBin::ConstSharedPtr msg); bool check_goal_footprint( const lanelet::ConstLanelet & current_lanelet, diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index eaadd019b462c..6f5ec16ddfc04 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -59,22 +59,23 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) lanelet::Points3d lefts; lanelet::Points3d rights; lanelet::Points3d centers; - std::vector bound_ids; + std::vector left_bound_ids; + std::vector right_bound_ids; for (const auto & llt : lanelets) { if (llt.id() != 0) { - bound_ids.push_back(llt.leftBound().id()); - bound_ids.push_back(llt.rightBound().id()); + left_bound_ids.push_back(llt.leftBound().id()); + right_bound_ids.push_back(llt.rightBound().id()); } } for (const auto & llt : lanelets) { - if (std::count(bound_ids.begin(), bound_ids.end(), llt.leftBound().id()) < 2) { + if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { for (const auto & pt : llt.leftBound()) { lefts.push_back(lanelet::Point3d(pt)); } } - if (std::count(bound_ids.begin(), bound_ids.end(), llt.rightBound().id()) < 2) { + if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { for (const auto & pt : llt.rightBound()) { rights.push_back(lanelet::Point3d(pt)); } diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 2e1ce6502441b..54aa3356909ef 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -153,6 +153,7 @@ struct DebugData std::vector mpt_ref_poses; std::vector lateral_errors; + std::vector yaw_errors; std::vector eb_traj; std::vector mpt_fixed_traj; diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 3225fc415e597..d26180f604e88 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -154,6 +154,9 @@ EBPathOptimizer::getOptimizedTrajectory( interpolated_points = std::vector( interpolated_points.begin(), interpolated_points.begin() + interpolated_points_end_seg_idx.get()); + if (interpolated_points.empty()) { + return boost::none; + } } } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index acc5ab823ca59..ceaaeeb865030 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -314,6 +314,15 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto return boost::none; } } + constexpr double max_yaw_deviation = 50.0 / 180 * 3.14; + for (const double yaw_error : debug_data.yaw_errors) { + if (max_yaw_deviation < std::abs(yaw_error)) { + RCLCPP_ERROR( + rclcpp::get_logger("mpt_optimizer"), + "return boost::none since yaw deviation is too large."); + return boost::none; + } + } auto full_optimized_ref_points = fixed_ref_points; full_optimized_ref_points.insert( @@ -1197,6 +1206,7 @@ std::vector MPTOptimizer::get ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); debug_data.mpt_ref_poses.push_back(ref_pose); debug_data.lateral_errors.push_back(lat_error); + debug_data.yaw_errors.push_back(yaw_error); ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i); if (i >= fixed_ref_points.size()) { diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index 66bd14c7c7fba..ad7257e907047 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -641,10 +641,10 @@ bool isOutsideDrivableArea( const auto left_start_point = getStartPoint(left_bound, right_bound.front()); const auto right_start_point = getStartPoint(right_bound, left_bound.front()); - // ignore point in front of the front line + // ignore point behind of the front line const std::vector front_bound = {left_start_point, right_start_point}; const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); - if (lat_dist_to_front_bound > min_dist) { + if (lat_dist_to_front_bound < -min_dist) { return false; } diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 4d114c47bfaf0..e3ddbfb1b157a 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -4,30 +4,29 @@ The `obstacle_cruise_planner` package has following modules. -- obstacle stop planning - - inserting a stop point in the trajectory when there is a static obstacle on the trajectory. -- adaptive cruise planning - - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory +- Stop planning + - stop when there is a static obstacle near the trajectory. +- Cruise planning + - slow down when there is a dynamic obstacle to cruise near the trajectory ## Interfaces ### Input topics -| Name | Type | Description | -| ----------------------------- | ----------------------------------------------- | --------------------------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory with smoothed velocity | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| Name | Type | Description | +| -------------------- | ----------------------------------------------- | ---------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | +| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics -| Name | Type | Description | -| ------------------------------ | ---------------------------------------------- | ------------------------------------- | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | -| `~output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | -| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | -| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | ------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | +| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | +| `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | ## Design @@ -56,15 +55,15 @@ struct ObstacleCruisePlannerData struct TargetObstacle { rclcpp::Time time_stamp; - bool orientation_reliable; geometry_msgs::msg::Pose pose; + bool orientation_reliable; + double velocity; bool velocity_reliable; - float velocity; - bool is_classified; ObjectClassification classification; - Shape shape; + std::string uuid; std::vector predicted_paths; - geometry_msgs::msg::Point collision_point; + std::vector collision_points; + bool has_stopped; }; ``` @@ -176,16 +175,37 @@ This includes not only cruising a front vehicle, but also reacting a cut-in and The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation. $$ -d = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, +d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, $$ -assuming that $d$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. +assuming that $d_rss$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`. | Parameter | Type | Description | | --------------------------------- | ------ | ----------------------------------------------------------------------------- | | `common.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] | -| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss] | +| `common.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] | +| `common.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] | + +The detailed formulation is as follows. + +$$ +d_{error} = d - d_{rss} \\ +d_{normalized} = lpf(d_{error} / d_{obstacle}) \\ +d_{quad, normalized} = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ +v_{pid} = pid(d_{quad, normalized}) \\ +v_{add} = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ +v_{target} = max(v_{ego} + v_{add}, v_{min, cruise}) +$$ + +| Variable | Description | +| ----------------- | --------------------------------------- | +| `d` | actual distane to obstacle | +| `d_{rss}` | ideal distance to obstacle based on RSS | +| `v_{min, cruise}` | `min_cruise_target_vel` | +| `w_{acc}` | `output_ratio_during_accel` | +| `lpf(val)` | apply low-pass filter to `val` | +| `pid(val)` | apply pid to `val` | ## Implementation diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 520637abed775..dfc2b2e6ddb44 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -3,15 +3,18 @@ common: planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - is_showing_debug_info: true + is_showing_debug_info: false + disable_stop_planning: false # true # longitudinal info - idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] @@ -25,7 +28,7 @@ bus: true trailer: true motorcycle: true - bicycle: false + bicycle: true pedestrian: false stop_obstacle_type: @@ -62,7 +65,7 @@ stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle ignored_outside_obstacle_type: - unknown: false + unknown: true car: false truck: false bus: false @@ -72,21 +75,41 @@ pedestrian: true pid_based_planner: - # use_predicted_obstacle_pose: false + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic - # PID gains to keep safe distance with the front vehicle - kp: 2.5 - ki: 0.0 - kd: 2.3 + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + disable_target_acceleration: true + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 - lpf_cruise_gain: 0.2 + lpf_normalized_error_cruise_dist_gain: 0.2 optimization_based_planner: dense_resampling_time_interval: 0.2 diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml new file mode 100644 index 0000000000000..e90c4f2a33c78 --- /dev/null +++ b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml @@ -0,0 +1,247 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.9 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.12 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.1 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/stop_planning_info/data.7 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.8 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.0 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.6 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.5 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.4 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.3 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/stop_planning_info/data.4 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.6 + + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/stop_planning_info/data.3 + + + + + diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml deleted file mode 100644 index 634268b93f2de..0000000000000 --- a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml +++ /dev/null @@ -1,150 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 724e7dbf1372b..3c35abe7fa14c 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -15,37 +15,16 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; - -namespace -{ -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} -} // namespace - struct TargetObstacle { TargetObstacle( @@ -58,9 +37,8 @@ struct TargetObstacle pose = object.kinematics.initial_pose_with_covariance.pose; velocity_reliable = true; velocity = aligned_velocity; - is_classified = true; classification = object.classification.at(0); - uuid = toHexString(object.object_id); + uuid = tier4_autoware_utils::toHexString(object.object_id); predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -72,11 +50,10 @@ struct TargetObstacle } rclcpp::Time time_stamp; - bool orientation_reliable; geometry_msgs::msg::Pose pose; + bool orientation_reliable; + double velocity; bool velocity_reliable; - float velocity; - bool is_classified; ObjectClassification classification; std::string uuid; std::vector predicted_paths; @@ -87,7 +64,7 @@ struct TargetObstacle struct ObstacleCruisePlannerData { rclcpp::Time current_time; - autoware_auto_planning_msgs::msg::Trajectory traj; + Trajectory traj; geometry_msgs::msg::Pose current_pose; double current_vel; double current_acc; @@ -97,6 +74,50 @@ struct ObstacleCruisePlannerData struct LongitudinalInfo { + explicit LongitudinalInfo(rclcpp::Node & node) + { + max_accel = node.declare_parameter("normal.max_acc"); + min_accel = node.declare_parameter("normal.min_acc"); + max_jerk = node.declare_parameter("normal.max_jerk"); + min_jerk = node.declare_parameter("normal.min_jerk"); + limit_max_accel = node.declare_parameter("limit.max_acc"); + limit_min_accel = node.declare_parameter("limit.min_acc"); + limit_max_jerk = node.declare_parameter("limit.max_jerk"); + limit_min_jerk = node.declare_parameter("limit.min_jerk"); + + idling_time = node.declare_parameter("common.idling_time"); + min_ego_accel_for_rss = node.declare_parameter("common.min_ego_accel_for_rss"); + min_object_accel_for_rss = node.declare_parameter("common.min_object_accel_for_rss"); + + safe_distance_margin = node.declare_parameter("common.safe_distance_margin"); + terminal_safe_distance_margin = + node.declare_parameter("common.terminal_safe_distance_margin"); + } + + void onParam(const std::vector & parameters) + { + tier4_autoware_utils::updateParam(parameters, "normal.max_accel", max_accel); + tier4_autoware_utils::updateParam(parameters, "normal.min_accel", min_accel); + tier4_autoware_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + tier4_autoware_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + tier4_autoware_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + tier4_autoware_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + + tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); + tier4_autoware_utils::updateParam( + parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); + tier4_autoware_utils::updateParam( + parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); + + tier4_autoware_utils::updateParam( + parameters, "common.safe_distance_margin", safe_distance_margin); + tier4_autoware_utils::updateParam( + parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); + } + + // common parameter double max_accel; double min_accel; double max_jerk; @@ -105,9 +126,13 @@ struct LongitudinalInfo double limit_min_accel; double limit_max_jerk; double limit_min_jerk; + + // rss parameter double idling_time; double min_ego_accel_for_rss; double min_object_accel_for_rss; + + // distance margin double safe_distance_margin; double terminal_safe_distance_margin; }; @@ -117,18 +142,21 @@ struct DebugData std::vector intentionally_ignored_obstacles; std::vector obstacles_to_stop; std::vector obstacles_to_cruise; - visualization_msgs::msg::MarkerArray stop_wall_marker; - visualization_msgs::msg::MarkerArray cruise_wall_marker; + MarkerArray stop_wall_marker; + MarkerArray cruise_wall_marker; std::vector detection_polygons; std::vector collision_points; }; struct EgoNearestParam { - EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold) - : dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold) + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node & node) { + dist_threshold = node.declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node.declare_parameter("ego_nearest_yaw_threshold"); } + double dist_threshold; double yaw_threshold; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 7570b1ad12274..e52c2671fea5e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -18,25 +18,12 @@ #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/accel_stamped.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include @@ -44,21 +31,6 @@ #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::AccelStamped; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_planning_msgs::msg::StopReasonArray; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; - namespace motion_planning { class ObstacleCruisePlannerNode : public rclcpp::Node @@ -70,9 +42,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // callback functions rcl_interfaces::msg::SetParametersResult onParam( const std::vector & parameters); - void onObjects(const PredictedObjects::ConstSharedPtr msg); - void onOdometry(const Odometry::ConstSharedPtr); - void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr); void onTrajectory(const Trajectory::ConstSharedPtr msg); void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); @@ -100,7 +69,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const PredictedObject & predicted_object, const Trajectory & traj, const bool is_driving_forward); void publishVelocityLimit(const boost::optional & vel_limit); - void publishDebugData(const DebugData & debug_data) const; + void publishDebugMarker(const DebugData & debug_data) const; + void publishDebugInfo() const; void publishCalculationTime(const double calculation_time) const; bool isCruiseObstacle(const uint8_t label); @@ -110,8 +80,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_showing_debug_info_; double min_behavior_stop_margin_; - double nearest_dist_deviation_threshold_; - double nearest_yaw_deviation_threshold_; double obstacle_velocity_threshold_from_cruise_to_stop_; double obstacle_velocity_threshold_from_stop_to_cruise_; @@ -125,26 +93,23 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr vel_limit_pub_; rclcpp::Publisher::SharedPtr clear_vel_limit_pub_; - rclcpp::Publisher::SharedPtr debug_marker_pub_; - rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; - rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber - rclcpp::Subscription::SharedPtr trajectory_sub_; - rclcpp::Subscription::SharedPtr smoothed_trajectory_sub_; + rclcpp::Subscription::SharedPtr traj_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr acc_sub_; - // self pose listener - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - // data for callback functions - PredictedObjects::ConstSharedPtr in_objects_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_; - - geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_; + PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr}; + Odometry::ConstSharedPtr current_odom_ptr_{nullptr}; + AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; @@ -160,7 +125,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node stop_watch_; // planner - std::unique_ptr planner_ptr_; + std::unique_ptr planner_ptr_{nullptr}; // previous closest obstacle std::shared_ptr prev_closest_obstacle_ptr_{nullptr}; @@ -193,8 +158,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node ObstacleFilteringParam obstacle_filtering_param_; bool need_to_clear_vel_limit_{false}; + EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; + bool disable_stop_planning_{false}; std::vector prev_target_obstacles_; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 609a09e00d219..bdd1bb5ee276e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -18,6 +18,7 @@ #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -25,20 +26,12 @@ #include #include -#include -#include - #include #include #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::Float32Stamped; - class OptimizationBasedPlanner : public PlannerInterface { public: @@ -77,8 +70,7 @@ class OptimizationBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::PointStamped & point); boost::optional calcTrajectoryLengthFromCurrentPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & current_pose); + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose); geometry_msgs::msg::Pose transformBaseLink2Center( const geometry_msgs::msg::Pose & pose_base_link); @@ -102,6 +94,11 @@ class OptimizationBasedPlanner : public PlannerInterface rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + // Subscriber + rclcpp::Subscription::SharedPtr smoothed_traj_sub_; + + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr}; + // Resampling Parameter double dense_resampling_time_interval_; double sparse_resampling_time_interval_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp new file mode 100644 index 0000000000000..3ab5dd0a011d9 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp @@ -0,0 +1,97 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class CruisePlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // cruise planning + CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, // index: 3 + CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, + CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + CRUISE_TARGET_OBSTACLE_DISTANCE, + CRUISE_ERROR_DISTANCE_RAW, + CRUISE_ERROR_DISTANCE_FILTERED, + + CRUISE_RELATIVE_EGO_VELOCITY, // index: 10 + CRUISE_TIME_TO_COLLISION, + + CRUISE_TARGET_VELOCITY, // index: 12 + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK_RATIO, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp deleted file mode 100644 index ae8a909d3bb51..0000000000000 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ -#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ - -#include - -class DebugValues -{ -public: - enum class TYPE { - // current - CURRENT_VELOCITY = 0, - CURRENT_ACCELERATION, - CURRENT_JERK, // ignored - // stop - STOP_CURRENT_OBJECT_DISTANCE = 3, - STOP_CURRENT_OBJECT_VELOCITY, - STOP_TARGET_OBJECT_DISTANCE, - STOP_TARGET_VELOCITY, // ignored - STOP_TARGET_ACCELERATION, - STOP_TARGET_JERK, // ignored - STOP_ERROR_OBJECT_DISTANCE, - // cruise - CRUISE_CURRENT_OBJECT_DISTANCE = 10, - CRUISE_CURRENT_OBJECT_VELOCITY, - CRUISE_TARGET_OBJECT_DISTANCE, - CRUISE_TARGET_VELOCITY, - CRUISE_TARGET_ACCELERATION, - CRUISE_TARGET_JERK, - CRUISE_ERROR_OBJECT_DISTANCE, - - SIZE - }; - - /** - * @brief get the index corresponding to the given value TYPE - * @param [in] type the TYPE enum for which to get the index - * @return index of the type - */ - int getValuesIdx(const TYPE type) const { return static_cast(type); } - /** - * @brief get all the debug values as an std::array - * @return array of all debug values - */ - std::array(TYPE::SIZE)> getValues() const { return values_; } - /** - * @brief set the given type to the given value - * @param [in] type TYPE of the value - * @param [in] value value to set - */ - void setValues(const TYPE type, const double val) { values_.at(static_cast(type)) = val; } - /** - * @brief set the given type to the given value - * @param [in] type index of the type - * @param [in] value value to set - */ - void setValues(const int type, const double val) { values_.at(type) = val; } - - void resetValues() { values_.fill(0.0); } - -private: - static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); - std::array(TYPE::SIZE)> values_; -}; - -#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index a6ed9d3e66bdf..f94516407e3c5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -15,13 +15,12 @@ #ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ #define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ -#include "obstacle_cruise_planner/pid_based_planner/debug_values.hpp" +#include "obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" #include "obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -29,26 +28,25 @@ #include #include -using tier4_debug_msgs::msg::Float32MultiArrayStamped; - class PIDBasedPlanner : public PlannerInterface { public: struct CruiseObstacleInfo { CruiseObstacleInfo( - const TargetObstacle & obstacle_arg, const double dist_to_cruise_arg, - const double normalized_dist_to_cruise_arg, double dist_to_obstacle_arg) + const TargetObstacle & obstacle_arg, const double error_cruise_dist_arg, + const double dist_to_obstacle_arg, const double target_dist_to_obstacle_arg) : obstacle(obstacle_arg), - dist_to_cruise(dist_to_cruise_arg), - normalized_dist_to_cruise(normalized_dist_to_cruise_arg), - dist_to_obstacle(dist_to_obstacle_arg) + error_cruise_dist(error_cruise_dist_arg), + dist_to_obstacle(dist_to_obstacle_arg), + target_dist_to_obstacle(target_dist_to_obstacle_arg) { } + TargetObstacle obstacle; - double dist_to_cruise; - double normalized_dist_to_cruise; + double error_cruise_dist; double dist_to_obstacle; + double target_dist_to_obstacle; }; PIDBasedPlanner( @@ -66,39 +64,85 @@ class PIDBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & cruise_obstacle_info); - void planCruise( + Float32MultiArrayStamped getCruisePlanningDebugMessage( + const rclcpp::Time & current_time) const override + { + return cruise_planning_debug_info_.convertToMessage(current_time); + } + +private: + boost::optional calcObstacleToCruise( + const ObstacleCruisePlannerData & planner_data); + Trajectory planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data); - VelocityLimit doCruise( - const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, - std::vector & debug_obstacles_to_cruise, - visualization_msgs::msg::MarkerArray & debug_walls_marker); - void publishDebugValues(const ObstacleCruisePlannerData & planner_data) const; + // velocity limit based planner + VelocityLimit doCruiseWithVelocityLimit( + const ObstacleCruisePlannerData & planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + + // velocityinsertion based planner + Trajectory doCruiseWithTrajectory( + const ObstacleCruisePlannerData & planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + Trajectory getAccelerationLimitedTrajectory( + const Trajectory traj, const geometry_msgs::msg::Pose & start_pose, const double v0, + const double a0, const double target_acc, const double target_jerk_ratio) const; // ROS parameters double min_accel_during_cruise_; - double vel_to_acc_weight_; double min_cruise_target_vel_; - // bool use_predicted_obstacle_pose_; - // pid controller - std::unique_ptr pid_controller_; - double output_ratio_during_accel_; + CruisePlanningDebugInfo cruise_planning_debug_info_; + + struct VelocityLimitBasedPlannerParam + { + std::unique_ptr pid_vel_controller; + double output_ratio_during_accel; + double vel_to_acc_weight; + bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; + }; + VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; + + struct VelocityInsertionBasedPlannerParam + { + std::unique_ptr pid_acc_controller; + std::unique_ptr pid_jerk_controller; + double output_acc_ratio_during_accel; + double output_jerk_ratio_during_accel; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityInsertionBasedPlannerParam velocity_insertion_based_planner_param_; // stop watch tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; - // publisher - rclcpp::Publisher::SharedPtr debug_values_pub_; + boost::optional prev_target_acc_; + + // lpf for nodes's input + std::shared_ptr lpf_dist_to_obstacle_ptr_; + std::shared_ptr lpf_error_cruise_dist_ptr_; + std::shared_ptr lpf_obstacle_vel_ptr_; + + // lpf for planner's input + std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; + + // lpf for output + std::shared_ptr lpf_output_vel_ptr_; + std::shared_ptr lpf_output_acc_ptr_; + std::shared_ptr lpf_output_jerk_ptr_; + + Trajectory prev_traj_; - boost::optional prev_target_vel_; + bool use_velocity_limit_based_planner_{true}; - DebugValues debug_values_; + double time_to_evaluate_rss_; - std::shared_ptr lpf_cruise_ptr_; + std::function error_func_; }; #endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 7fe64181ff420..06d7710290081 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -17,30 +17,16 @@ #include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/common_structs.hpp" +#include "obstacle_cruise_planner/stop_planning_debug_info.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/stop_reason_array.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "visualization_msgs/msg/marker_array.hpp" #include #include #include -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::StopSpeedExceeded; -using tier4_planning_msgs::msg::VelocityLimit; - class PlannerInterface { public: @@ -51,8 +37,7 @@ class PlannerInterface vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param) { - stop_reasons_pub_ = - node.create_publisher("~/output/stop_reasons", 1); + stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = @@ -61,14 +46,10 @@ class PlannerInterface PlannerInterface() = default; - void setParams( - const bool is_showing_debug_info, const double min_behavior_stop_margin, - const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold) + void setParam(const bool is_showing_debug_info, const double min_behavior_stop_margin) { is_showing_debug_info_ = is_showing_debug_info; min_behavior_stop_margin_ = min_behavior_stop_margin; - nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold; - nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold; } Trajectory generateStopTrajectory( @@ -78,31 +59,20 @@ class PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, DebugData & debug_data) = 0; - void updateCommonParam(const std::vector & parameters) + void onParam(const std::vector & parameters) { - auto & i = longitudinal_info_; - - tier4_autoware_utils::updateParam(parameters, "common.max_accel", i.max_accel); - tier4_autoware_utils::updateParam(parameters, "common.min_accel", i.min_accel); - tier4_autoware_utils::updateParam(parameters, "common.max_jerk", i.max_jerk); - tier4_autoware_utils::updateParam(parameters, "common.min_jerk", i.min_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.max_accel", i.limit_max_accel); - tier4_autoware_utils::updateParam(parameters, "limit.min_accel", i.limit_min_accel); - tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", i.limit_max_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", i.limit_min_jerk); - tier4_autoware_utils::updateParam( - parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss); - tier4_autoware_utils::updateParam( - parameters, "common.min_object_accel_for_rss", i.min_object_accel_for_rss); - tier4_autoware_utils::updateParam(parameters, "common.idling_time", i.idling_time); + updateCommonParam(parameters); + updateParam(parameters); } - virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} - - // TODO(shimizu) remove this function - void setSmoothedTrajectory(const Trajectory::ConstSharedPtr traj) + Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const + { + return stop_planning_debug_info_.convertToMessage(current_time); + } + virtual Float32MultiArrayStamped getCruisePlanningDebugMessage( + [[maybe_unused]] const rclcpp::Time & current_time) const { - smoothed_trajectory_ptr_ = traj; + return Float32MultiArrayStamped{}; } protected: @@ -110,11 +80,9 @@ class PlannerInterface bool is_showing_debug_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; - double nearest_dist_deviation_threshold_; - double nearest_yaw_deviation_threshold_; // Publishers - rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; @@ -123,8 +91,8 @@ class PlannerInterface EgoNearestParam ego_nearest_param_; - // TODO(shimizu) remove these parameters - Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; + // debug info + StopPlanningDebugInfo stop_planning_debug_info_; double calcDistanceToCollisionPoint( const ObstacleCruisePlannerData & planner_data, @@ -140,6 +108,13 @@ class PlannerInterface return rss_dist_with_margin; } + void updateCommonParam(const std::vector & parameters) + { + longitudinal_info_.onParam(parameters); + } + + virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} + size_t findEgoIndex(const Trajectory & traj, const geometry_msgs::msg::Pose & ego_pose) const { const auto traj_points = motion_utils::convertToTrajectoryPointArray(traj); diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 651c63d4593a5..6e468bd82defa 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -15,14 +15,10 @@ #ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ #define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/pose.hpp" - #include #include @@ -36,39 +32,36 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; boost::optional getCollisionIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & shape, + const Trajectory & traj, const std::vector & traj_polygons, + const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape, std::vector & collision_points, const double max_dist = std::numeric_limits::max()); std::vector getCollisionPoints( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); std::vector willCollideWithSurroundObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, - const double max_dist, const double ego_obstacle_overlap_time_threshold, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double max_dist, + const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward); std::vector createOneStepPolygons( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, + const double expand_width); geometry_msgs::msg::PointStamped calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, - const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, - const double vehicle_max_longitudinal_offset, const bool is_driving_forward); + const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset, + const bool is_driving_forward); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp new file mode 100644 index 0000000000000..7c682a1f63c01 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp @@ -0,0 +1,88 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#define OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class StopPlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // stop planning + STOP_CURRENT_OBSTACLE_DISTANCE, // index: 3 + STOP_CURRENT_OBSTACLE_VELOCITY, + STOP_TARGET_OBSTACLE_DISTANCE, + STOP_TARGET_VELOCITY, + STOP_TARGET_ACCELERATION, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp new file mode 100644 index 0000000000000..7f42512646f3c --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -0,0 +1,61 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ +#define OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ + +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "tier4_planning_msgs/msg/stop_factor.hpp" +#include "tier4_planning_msgs/msg/stop_reason_array.hpp" +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::AccelStamped; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using tier4_planning_msgs::msg::StopSpeedExceeded; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +#endif // OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 553f4ecfcfe9b..b509192d34371 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -17,18 +17,11 @@ #include "common_structs.hpp" #include "motion_utils/motion_utils.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include @@ -37,36 +30,27 @@ namespace obstacle_cruise_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; - -bool isVehicle(const uint8_t label); - -visualization_msgs::msg::Marker getObjectMarker( +Marker getObjectMarker( const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, const double r, const double g, const double b); boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, - const double target_length); + const Trajectory & traj, const size_t start_idx, const double target_length); boost::optional getCurrentObjectPoseFromPredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); boost::optional getCurrentObjectPoseFromPredictedPaths( - const std::vector & predicted_paths, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); geometry_msgs::msg::PoseStamped getCurrentObjectPose( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, - const std_msgs::msg::Header & obj_header, const rclcpp::Time & current_time, - const bool use_prediction); + const PredictedObject & predicted_object, const std_msgs::msg::Header & obj_header, + const rclcpp::Time & current_time, const bool use_prediction); boost::optional getClosestStopObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & target_obstacles); - -std::string toHexString(const unique_identifier_msgs::msg::UUID & id); + const Trajectory & traj, const std::vector & target_obstacles); template size_t getIndexWithLongitudinalOffset( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 39a4dd5de0815..8b344affd9c7f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -29,7 +29,7 @@ namespace { -VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time & current_time) +VelocityLimitClearCommand createVelocityLimitClearCommandMessage(const rclcpp::Time & current_time) { VelocityLimitClearCommand msg; msg.stamp = current_time; @@ -38,23 +38,11 @@ VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time return msg; } -// TODO(murooka) make this function common -size_t findExtendedNearestIndex( - const Trajectory traj, const geometry_msgs::msg::Pose & pose, const double max_dist, - const double max_yaw) -{ - const auto nearest_idx = motion_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw); - if (nearest_idx) { - return nearest_idx.get(); - } - return motion_utils::findNearestIndex(traj.points, pose.position); -} - -Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx) +Trajectory trimTrajectoryFrom(const Trajectory & input, const double start_idx) { Trajectory output{}; - for (size_t i = nearest_idx; i < input.points.size(); ++i) { + for (size_t i = start_idx; i < input.points.size(); ++i) { output.points.push_back(input.points.at(i)); } @@ -134,13 +122,13 @@ double calcAlignedAdaptiveCruise( return object_vel * std::cos(object_yaw - traj_yaw); } -double calcObjectMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObjectMaxLength(const Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -194,27 +182,23 @@ namespace motion_planning { ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), - self_pose_listener_(this), - in_objects_ptr_(nullptr), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) { using std::placeholders::_1; // subscriber - trajectory_sub_ = create_subscription( + traj_sub_ = create_subscription( "~/input/trajectory", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); - smoothed_trajectory_sub_ = create_subscription( - "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onSmoothedTrajectory, this, _1)); objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onObjects, this, _1)); + "~/input/objects", rclcpp::QoS{1}, + [this](const PredictedObjects::ConstSharedPtr msg) { in_objects_ptr_ = msg; }); odom_sub_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1)); + [this](const Odometry::ConstSharedPtr msg) { current_odom_ptr_ = msg; }); acc_sub_ = create_subscription( "~/input/acceleration", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onAccel, this, std::placeholders::_1)); + [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { current_accel_ptr_ = msg; }); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -222,57 +206,23 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & create_publisher("~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); clear_vel_limit_pub_ = create_publisher( "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); + + // debug publisher debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); - debug_cruise_wall_marker_pub_ = - create_publisher("~/debug/cruise/virtual_wall", 1); - debug_stop_wall_marker_pub_ = - create_publisher("~/virtual_wall", 1); - debug_marker_pub_ = create_publisher("~/debug/marker", 1); - - // longitudinal_info - const auto longitudinal_info = [&]() { - const double max_accel = declare_parameter("normal.max_acc"); - const double min_accel = declare_parameter("normal.min_acc"); - const double max_jerk = declare_parameter("normal.max_jerk"); - const double min_jerk = declare_parameter("normal.min_jerk"); - const double limit_max_accel = declare_parameter("limit.max_acc"); - const double limit_min_accel = declare_parameter("limit.min_acc"); - const double limit_max_jerk = declare_parameter("limit.max_jerk"); - const double limit_min_jerk = declare_parameter("limit.min_jerk"); - - const double min_ego_accel_for_rss = declare_parameter("common.min_ego_accel_for_rss"); - const double min_object_accel_for_rss = - declare_parameter("common.min_object_accel_for_rss"); - const double idling_time = declare_parameter("common.idling_time"); - const double safe_distance_margin = declare_parameter("common.safe_distance_margin"); - const double terminal_safe_distance_margin = - declare_parameter("common.terminal_safe_distance_margin"); - - return LongitudinalInfo{ - max_accel, - min_accel, - max_jerk, - min_jerk, - limit_max_accel, - limit_min_accel, - limit_max_jerk, - limit_min_jerk, - idling_time, - min_ego_accel_for_rss, - min_object_accel_for_rss, - safe_distance_margin, - terminal_safe_distance_margin}; - }(); + debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); + debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); + debug_marker_pub_ = create_publisher("~/debug/marker", 1); + debug_stop_planning_info_pub_ = + create_publisher("~/debug/stop_planning_info", 1); + debug_cruise_planning_info_pub_ = + create_publisher("~/debug/cruise_planning_info", 1); - const auto ego_nearest_param = [&]() { - const double ego_nearest_dist_threshold = - declare_parameter("ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + const auto longitudinal_info = LongitudinalInfo(*this); - return EgoNearestParam(ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - }(); + ego_nearest_param_ = EgoNearestParam(*this); is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); + disable_stop_planning_ = declare_parameter("common.disable_stop_planning"); { // Obstacle filtering parameters obstacle_filtering_param_.rough_detection_area_expand_width = @@ -349,26 +299,20 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) { planner_ptr_ = std::make_unique( - *this, longitudinal_info, vehicle_info_, ego_nearest_param); + *this, longitudinal_info, vehicle_info_, ego_nearest_param_); } else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) { planner_ptr_ = std::make_unique( - *this, longitudinal_info, vehicle_info_, ego_nearest_param); + *this, longitudinal_info, vehicle_info_, ego_nearest_param_); } else { std::logic_error("Designated algorithm is not supported."); } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); - nearest_dist_deviation_threshold_ = - declare_parameter("common.nearest_dist_deviation_threshold"); - nearest_yaw_deviation_threshold_ = - declare_parameter("common.nearest_yaw_deviation_threshold"); obstacle_velocity_threshold_from_cruise_to_stop_ = declare_parameter("common.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise_ = declare_parameter("common.obstacle_velocity_threshold_from_stop_to_cruise"); - planner_ptr_->setParams( - is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_); } { // cruise obstacle type @@ -425,9 +369,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } } - // wait for first self pose - self_pose_listener_.waitForFirstPose(); - // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); @@ -447,14 +388,14 @@ ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlann rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( const std::vector & parameters) { - planner_ptr_->updateCommonParam(parameters); - planner_ptr_->updateParam(parameters); + planner_ptr_->onParam(parameters); tier4_autoware_utils::updateParam( parameters, "common.is_showing_debug_info", is_showing_debug_info_); - planner_ptr_->setParams( - is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_); + + tier4_autoware_utils::updateParam( + parameters, "common.disable_stop_planning", disable_stop_planning_); // obstacle_filtering tier4_autoware_utils::updateParam( @@ -506,59 +447,34 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( return result; } -void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr msg) -{ - in_objects_ptr_ = msg; -} - -void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) -{ - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = msg->header; - current_twist_ptr_->twist = msg->twist.twist; -} - -void ObstacleCruisePlannerNode::onAccel(const AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - current_accel_ptr_ = std::make_unique(); - current_accel_ptr_->header = msg->header; - current_accel_ptr_->accel = msg->accel.accel; -} - -void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - planner_ptr_->setSmoothedTrajectory(msg); -} - void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); - // check if subscribed variables are ready - if (msg->points.empty() || !current_twist_ptr_ || !in_objects_ptr_ || !current_pose_ptr) { + if (msg->points.empty() || !current_odom_ptr_ || !in_objects_ptr_) { return; } stop_watch_.tic(__func__); + const auto current_pose = current_odom_ptr_->pose.pose; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + // Get Target Obstacles DebugData debug_data; const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; - const auto target_obstacles = getTargetObstacles( - *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, is_driving_forward_, - debug_data); + const auto target_obstacles = + getTargetObstacles(*msg, current_pose, current_vel, is_driving_forward_, debug_data); // create data for stop - const auto stop_data = - createStopData(*msg, current_pose_ptr->pose, target_obstacles, is_driving_forward_); + const auto stop_data = createStopData(*msg, current_pose, target_obstacles, is_driving_forward_); // stop planning const auto stop_traj = planner_ptr_->generateStopTrajectory(stop_data, debug_data); // create data for cruise const auto cruise_data = - createCruiseData(stop_traj, current_pose_ptr->pose, target_obstacles, is_driving_forward_); + createCruiseData(stop_traj, current_pose, target_obstacles, is_driving_forward_); // cruise planning boost::optional vel_limit; @@ -572,7 +488,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // publish debug data - publishDebugData(debug_data); + publishDebugMarker(debug_data); + publishDebugInfo(); // publish and print calculation time const double calculation_time = stop_watch_.toc(__func__); @@ -599,8 +516,8 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); - const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = current_accel_ptr_->accel.linear.x; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + const double current_accel = current_accel_ptr_->accel.accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -610,9 +527,10 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( planner_data.current_vel = current_vel; planner_data.current_acc = current_accel; planner_data.is_driving_forward = is_driving_forward; + for (const auto & obstacle : obstacles) { // consider all target obstacles when driving backward - if (!planner_data.is_driving_forward || obstacle.has_stopped) { + if (!disable_stop_planning_ && (!planner_data.is_driving_forward || obstacle.has_stopped)) { planner_data.target_obstacles.push_back(obstacle); } } @@ -639,8 +557,8 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); - const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = current_accel_ptr_->accel.linear.x; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + const double current_accel = current_accel_ptr_->accel.accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -651,7 +569,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( planner_data.current_acc = current_accel; planner_data.is_driving_forward = is_driving_forward; for (const auto & obstacle : obstacles) { - if (planner_data.is_driving_forward && !obstacle.has_stopped) { + if (disable_stop_planning_ || (planner_data.is_driving_forward && !obstacle.has_stopped)) { planner_data.target_obstacles.push_back(obstacle); } } @@ -685,8 +603,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto current_time = now(); const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp); - const size_t ego_idx = findExtendedNearestIndex( - traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + const size_t ego_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); // calculate decimated trajectory const auto trimmed_traj = trimTrajectoryFrom(traj, ego_idx); @@ -706,7 +624,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( std::vector target_obstacles; for (const auto & predicted_object : predicted_objects.objects) { - const auto object_id = toHexString(predicted_object.object_id).substr(0, 4); + const auto object_id = + tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); // filter object whose label is not cruised or stopped const bool is_target_obstacle = isStopObstacle(predicted_object.classification.front().label) || @@ -931,7 +850,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { - return obstacle_cruise_utils::toHexString(predicted_object.object_id) == + return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_obstacle_ptr_->uuid; }); @@ -1013,23 +932,28 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( const boost::optional & vel_limit) { if (vel_limit) { + // publish velocity limit vel_limit_pub_->publish(vel_limit.get()); need_to_clear_vel_limit_ = true; - } else { - if (need_to_clear_vel_limit_) { - const auto clear_vel_limit_msg = createVelocityLimitClearCommandMsg(now()); - clear_vel_limit_pub_->publish(clear_vel_limit_msg); - need_to_clear_vel_limit_ = false; - } + return; } + + if (!need_to_clear_vel_limit_) { + return; + } + + // clear velocity limit + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now()); + clear_vel_limit_pub_->publish(clear_vel_limit_msg); + need_to_clear_vel_limit_ = false; } -void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) const +void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) const { stop_watch_.tic(__func__); - visualization_msgs::msg::MarkerArray debug_marker; - const auto current_time = now(); + // 1. publish debug marker + MarkerArray debug_marker; // obstacles to cruise for (size_t i = 0; i < debug_data.obstacles_to_cruise.size(); ++i) { @@ -1055,7 +979,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c { // footprint polygons auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + "map", now(), "detection_polygons", 0, Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); @@ -1077,7 +1001,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c { // collision points for (size_t i = 0; i < debug_data.collision_points.size(); ++i) { auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "collision_points", i, visualization_msgs::msg::Marker::SPHERE, + "map", now(), "collision_points", i, Marker::SPHERE, tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); marker.pose.position = debug_data.collision_points.at(i); @@ -1087,17 +1011,28 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c debug_marker_pub_->publish(debug_marker); - // wall for cruise and stop + // 2. publish virtual wall for cruise and stop debug_cruise_wall_marker_pub_->publish(debug_data.cruise_wall_marker); debug_stop_wall_marker_pub_->publish(debug_data.stop_wall_marker); - // print calculation time + // 3. print calculation time const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", __func__, calculation_time); } +void ObstacleCruisePlannerNode::publishDebugInfo() const +{ + // stop + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // cruise + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); +} + void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const { Float32Stamped calculation_time_msg; @@ -1106,5 +1041,6 @@ void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_ debug_calculation_time_pub_->publish(calculation_time_msg); } } // namespace motion_planning + #include RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::ObstacleCruisePlannerNode) diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 8948cd2de8eb3..262e2b619d36e 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -39,6 +39,10 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param) { + smoothed_traj_sub_ = node.create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; }); + // parameter dense_resampling_time_interval_ = node.declare_parameter("optimization_based_planner.dense_resampling_time_interval"); @@ -85,8 +89,7 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); boundary_pub_ = node.create_publisher("~/boundary", 1); - debug_wall_marker_pub_ = - node.create_publisher("~/debug/wall_marker", 1); + debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); } Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( @@ -284,8 +287,8 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const auto & input_traj = planner_data.traj; const double vehicle_speed{std::abs(current_vel)}; const auto current_closest_point = motion_utils::calcInterpolatedPoint( - input_traj, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + input_traj, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)}; double initial_vel{}; @@ -301,11 +304,11 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( TrajectoryPoint prev_output_closest_point; if (smoothed_trajectory_ptr_) { prev_output_closest_point = motion_utils::calcInterpolatedPoint( - *smoothed_trajectory_ptr_, current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + *smoothed_trajectory_ptr_, current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); } else { prev_output_closest_point = motion_utils::calcInterpolatedPoint( - prev_traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + prev_traj, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); } // when velocity tracking deviation is large @@ -328,8 +331,8 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint( - input_traj.points, current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + input_traj.points, current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { initial_vel = engage_velocity_; initial_acc = engage_acceleration_; @@ -363,8 +366,8 @@ bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerDa { // If goal is close and current velocity is low, we don't optimize trajectory const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_data.traj.points, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) { return true; } @@ -437,7 +440,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( planner_data.traj.points, planner_data.current_pose.position, min_slow_down_point_length); if (marker_pose) { - visualization_msgs::msg::MarkerArray wall_msg; + MarkerArray wall_msg; if (obj.has_stopped) { const auto markers = motion_utils::createStopVirtualWallMarker( @@ -590,19 +593,18 @@ bool OptimizationBasedPlanner::checkOnTrajectory( } boost::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & current_pose) + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose) { const auto traj_length = motion_utils::calcSignedArcLength( - traj.points, current_pose, traj.points.size() - 1, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + traj.points, current_pose, traj.points.size() - 1, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (!traj_length) { return {}; } const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint( - traj.points, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_closest_stop_point) { return std::min(traj_length.get(), dist_to_closest_stop_point.get()); } diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 267ff1f4502f2..7401e3d0fe6ee 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,6 +14,7 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "interpolation/spline_interpolation.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -40,15 +41,15 @@ VelocityLimit createVelocityLimitMsg( return msg; } -Float32MultiArrayStamped convertDebugValuesToMsg( - const rclcpp::Time & current_time, const DebugValues & debug_values) +template +T getSign(const T val) { - Float32MultiArrayStamped debug_msg{}; - debug_msg.stamp = current_time; - for (const auto & v : debug_values.getValues()) { - debug_msg.data.push_back(v); + if (0 < val) { + return static_cast(1); + } else if (val < 0) { + return static_cast(-1); } - return debug_msg; + return static_cast(0); } } // namespace @@ -60,30 +61,90 @@ PIDBasedPlanner::PIDBasedPlanner( min_accel_during_cruise_ = node.declare_parameter("pid_based_planner.min_accel_during_cruise"); - // pid controller - const double kp = node.declare_parameter("pid_based_planner.kp"); - const double ki = node.declare_parameter("pid_based_planner.ki"); - const double kd = node.declare_parameter("pid_based_planner.kd"); - pid_controller_ = std::make_unique(kp, ki, kd); - output_ratio_during_accel_ = - node.declare_parameter("pid_based_planner.output_ratio_during_accel"); - - // some parameters - // use_predicted_obstacle_pose_ = - // node.declare_parameter("pid_based_planner.use_predicted_obstacle_pose"); + use_velocity_limit_based_planner_ = + node.declare_parameter("pid_based_planner.use_velocity_limit_based_planner"); + + { // velocity limit based planner + const double kp = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.kp"); + const double ki = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.ki"); + const double kd = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.kd"); + velocity_limit_based_planner_param_.pid_vel_controller = + std::make_unique(kp, ki, kd); + + velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel"); + velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.vel_to_acc_weight"); + + velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); + + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); + } - vel_to_acc_weight_ = node.declare_parameter("pid_based_planner.vel_to_acc_weight"); + { // velocity insertion based planner + // pid controller for acc + const double kp_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kp_acc"); + const double ki_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.ki_acc"); + const double kd_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kd_acc"); + velocity_insertion_based_planner_param_.pid_acc_controller = + std::make_unique(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + const double kp_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kp_jerk"); + const double ki_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.ki_jerk"); + const double kd_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kd_jerk"); + velocity_insertion_based_planner_param_.pid_jerk_controller = + std::make_unique(kp_jerk, ki_jerk, kd_jerk); + + velocity_insertion_based_planner_param_.output_acc_ratio_during_accel = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel"); + velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel"); + + velocity_insertion_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc"); + } min_cruise_target_vel_ = node.declare_parameter("pid_based_planner.min_cruise_target_vel"); + time_to_evaluate_rss_ = node.declare_parameter("pid_based_planner.time_to_evaluate_rss"); + const auto error_function_type = + node.declare_parameter("pid_based_planner.error_function_type"); + error_func_ = [&]() -> std::function { + if (error_function_type == "linear") { + return [](const double val) { return val; }; + } else if (error_function_type == "quadratic") { + return [](const double val) { return getSign(val) * std::pow(val, 2); }; + } + throw std::domain_error("error function type is not supported"); + }(); // low pass filter - const double lpf_cruise_gain = - node.declare_parameter("pid_based_planner.lpf_cruise_gain"); - lpf_cruise_ptr_ = std::make_shared(lpf_cruise_gain); - - // publisher - debug_values_pub_ = node.create_publisher("~/debug/values", 1); + const double lpf_normalized_error_cruise_dist_gain = + node.declare_parameter("pid_based_planner.lpf_normalized_error_cruise_dist_gain"); + lpf_normalized_error_cruise_dist_ptr_ = + std::make_shared(lpf_normalized_error_cruise_dist_gain); + lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); + lpf_obstacle_vel_ptr_ = std::make_shared(0.5); + lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_vel_ptr_ = std::make_shared(0.5); + lpf_output_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); } Trajectory PIDBasedPlanner::generateCruiseTrajectory( @@ -91,36 +152,35 @@ Trajectory PIDBasedPlanner::generateCruiseTrajectory( DebugData & debug_data) { stop_watch_.tic(__func__); - debug_values_.resetValues(); + cruise_planning_debug_info_.reset(); // calc obstacles to cruise - boost::optional cruise_obstacle_info; - calcObstaclesToCruise(planner_data, cruise_obstacle_info); + const auto cruise_obstacle_info = calcObstacleToCruise(planner_data); // plan cruise - planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); - - // publish debug values - publishDebugValues(planner_data); + const auto cruise_traj = planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, " %s := %f [ms]", __func__, calculation_time); - return planner_data.traj; + prev_traj_ = cruise_traj; + return cruise_traj; } -void PIDBasedPlanner::calcObstaclesToCruise( - const ObstacleCruisePlannerData & planner_data, - boost::optional & cruise_obstacle_info) +boost::optional PIDBasedPlanner::calcObstacleToCruise( + const ObstacleCruisePlannerData & planner_data) { - debug_values_.setValues(DebugValues::TYPE::CURRENT_VELOCITY, planner_data.current_vel); - debug_values_.setValues(DebugValues::TYPE::CURRENT_ACCELERATION, planner_data.current_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, planner_data.current_acc); auto modified_target_obstacles = planner_data.target_obstacles; // search highest probability obstacle for cruise + boost::optional cruise_obstacle_info; for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { const auto & obstacle = planner_data.target_obstacles.at(o_idx); @@ -129,35 +189,102 @@ void PIDBasedPlanner::calcObstaclesToCruise( } // NOTE: from ego's front to obstacle's back + // TODO(murooka) this is not dist to obstacle const double dist_to_obstacle = - calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point); - - if (!obstacle.has_stopped) { // cruise - // calculate distance between ego and obstacle based on RSS - const double rss_dist = calcRSSDistance( - planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); - - // calculate error distance and normalized one - const double error_dist = dist_to_obstacle - rss_dist; - if (cruise_obstacle_info) { - if (error_dist > cruise_obstacle_info->dist_to_cruise) { - continue; - } + calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) + + (obstacle.velocity - planner_data.current_vel) * time_to_evaluate_rss_; + + // calculate distance between ego and obstacle based on RSS + const double target_dist_to_obstacle = calcRSSDistance( + planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle; + if (cruise_obstacle_info) { + if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { + continue; } - const double normalized_dist_to_cruise = error_dist / dist_to_obstacle; - cruise_obstacle_info = - CruiseObstacleInfo(obstacle, error_dist, normalized_dist_to_cruise, dist_to_obstacle); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_VELOCITY, obstacle.velocity); - debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_DISTANCE, dist_to_obstacle); - debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_OBJECT_DISTANCE, rss_dist); - debug_values_.setValues(DebugValues::TYPE::CRUISE_ERROR_OBJECT_DISTANCE, error_dist); } + cruise_obstacle_info = + CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle); } + + if (!cruise_obstacle_info) { // if obstacle to cruise was not found + lpf_dist_to_obstacle_ptr_->reset(); + lpf_obstacle_vel_ptr_->reset(); + lpf_error_cruise_dist_ptr_->reset(); + return {}; + } + + // if obstacle to cruise was found + { // debug data + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY, + planner_data.current_vel - cruise_obstacle_info->obstacle.velocity); + const double non_zero_relative_vel = [&]() { + const double relative_vel = + planner_data.current_vel - cruise_obstacle_info->obstacle.velocity; + constexpr double epsilon = 0.001; + if (epsilon < std::abs(relative_vel)) { + return relative_vel; + } + return getSign(relative_vel) * epsilon; + }(); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION, + cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel); + } + + { // dist to obstacle + const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const double filtered_dist_to_obstacle = + lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + filtered_dist_to_obstacle); + + cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle; + } + + { // obstacle velocity + const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity; + const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + filtered_obstacle_velocity); + + cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity; + } + + { // error distance for cruising + const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double filtered_error_cruise_dist = + lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist); + + cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist; + } + + { // target dist for cruising + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE, + cruise_obstacle_info->target_dist_to_obstacle); + } + + return cruise_obstacle_info; } -void PIDBasedPlanner::planCruise( +Trajectory PIDBasedPlanner::planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data) { @@ -167,92 +294,321 @@ void PIDBasedPlanner::planCruise( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, "cruise planning"); - vel_limit = doCruise( - planner_data, cruise_obstacle_info.get(), debug_data.obstacles_to_cruise, - debug_data.cruise_wall_marker); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_VELOCITY, vel_limit->max_velocity); - debug_values_.setValues( - DebugValues::TYPE::CRUISE_TARGET_ACCELERATION, vel_limit->constraints.min_acceleration); - } else { - // reset previous target velocity if adaptive cruise is not enabled - prev_target_vel_ = {}; - lpf_cruise_ptr_->reset(); - - // delete marker - const auto markers = - motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + { // update debug marker + // virtual wall marker for cruise + const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const size_t ego_idx = findEgoIndex(planner_data.traj, planner_data.current_pose); + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + const double dist_to_rss_wall = + std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, dist_to_rss_wall, ego_idx); + + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + planner_data.traj.points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, + 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + + // cruise obstacle + debug_data.obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + } + + // do cruise planning + if (!use_velocity_limit_based_planner_) { + const auto cruise_traj = doCruiseWithTrajectory(planner_data, cruise_obstacle_info.get()); + return cruise_traj; + } + + vel_limit = doCruiseWithVelocityLimit(planner_data, cruise_obstacle_info.get()); + return planner_data.traj; } + + // reset previous cruise data if adaptive cruise is not enabled + prev_target_acc_ = {}; + lpf_normalized_error_cruise_dist_ptr_->reset(); + lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); + lpf_output_jerk_ptr_->reset(); + + // delete marker + const auto markers = + motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + + return planner_data.traj; } -VelocityLimit PIDBasedPlanner::doCruise( - const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, - std::vector & debug_obstacles_to_cruise, - visualization_msgs::msg::MarkerArray & debug_wall_marker) +VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) { - const double dist_to_cruise = cruise_obstacle_info.dist_to_cruise; - const double filtered_normalized_dist_to_cruise = [&]() { - const double normalized_dist_to_cruise = cruise_obstacle_info.normalized_dist_to_cruise; - return lpf_cruise_ptr_->filter(normalized_dist_to_cruise); + const auto & p = velocity_limit_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); }(); - const double dist_to_obstacle = cruise_obstacle_info.dist_to_obstacle; - const size_t ego_idx = findEgoIndex(planner_data.traj, planner_data.current_pose); + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); // calculate target velocity with acceleration limit by PID controller - const double pid_output_vel = pid_controller_->calc(filtered_normalized_dist_to_cruise); - [[maybe_unused]] const double prev_vel = - prev_target_vel_ ? prev_target_vel_.get() : planner_data.current_vel; - + const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist); const double additional_vel = [&]() { - if (filtered_normalized_dist_to_cruise > 0) { - return pid_output_vel * output_ratio_during_accel_; + if (modified_error_cruise_dist > 0) { + return pid_output_vel * p.output_ratio_during_accel; } return pid_output_vel; }(); - const double positive_target_vel = - std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); + const double positive_target_vel = lpf_output_vel_ptr_->filter( + std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel)); // calculate target acceleration - const double target_acc = vel_to_acc_weight_ * additional_vel; - const double target_acc_with_acc_limit = - std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); + const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + + double target_acc = p.vel_to_acc_weight * additional_vel; + + // apply acc limit + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return lpf_output_acc_ptr_->filter(target_acc); + } + + if (p.enable_jerk_limit_to_output_acc) { // apply jerk limit + const double jerk = (target_acc - prev_target_acc_.get()) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = prev_target_acc_.get() + limited_jerk * 0.1; + } + + return lpf_output_acc_ptr_->filter(target_acc); + }(); + prev_target_acc_ = target_acc; + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, "target_velocity %f", positive_target_vel); - prev_target_vel_ = positive_target_vel; - // set target longitudinal motion const auto vel_limit = createVelocityLimitMsg( - planner_data.current_time, positive_target_vel, target_acc_with_acc_limit, - longitudinal_info_.max_jerk, longitudinal_info_.min_jerk); + planner_data.current_time, positive_target_vel, target_acc, longitudinal_info_.max_jerk, + longitudinal_info_.min_jerk); - // virtual wall marker for cruise - const double abs_ego_offset = planner_data.is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - const double dist_to_rss_wall = - std::min(dist_to_cruise + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + return vel_limit; +} - const auto markers = motion_utils::createSlowDownVirtualWallMarker( - planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, - dist_to_rss_wall); - tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); +Trajectory PIDBasedPlanner::doCruiseWithTrajectory( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_insertion_based_planner_param_; - debug_obstacles_to_cruise.push_back(cruise_obstacle_info.obstacle); + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); - return vel_limit; + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + // calculate target acceleration + const double target_acc = [&]() { + double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_acc *= p.output_acc_ratio_during_accel; + } + + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return target_acc; + } + + if (p.enable_jerk_limit_to_output_acc) { + const double jerk = (target_acc - prev_target_acc_.get()) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = prev_target_acc_.get() + limited_jerk * 0.1; + } + + return target_acc; + }(); + prev_target_acc_ = target_acc; + + const double target_jerk_ratio = [&]() { + double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_jerk_ratio *= p.output_jerk_ratio_during_accel; + } + + target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); + return target_jerk_ratio; + }(); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio); + + // set target longitudinal motion + const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { + // if (smoothed_trajectory_ptr_) { + // return motion_utils::calcInterpolatedPoint( + // *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_, + // nearest_yaw_deviation_threshold_); + // } + return motion_utils::calcInterpolatedPoint( + prev_traj_, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); + }(); + const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; + const double a0 = prev_traj_closest_point.acceleration_mps2; + + auto cruise_traj = getAccelerationLimitedTrajectory( + planner_data.traj, planner_data.current_pose, v0, a0, target_acc, target_jerk_ratio); + + const auto zero_vel_idx_opt = motion_utils::searchZeroVelocityIndex(cruise_traj.points); + if (!zero_vel_idx_opt) { + return cruise_traj; + } + + for (size_t i = zero_vel_idx_opt.get(); i < cruise_traj.points.size(); ++i) { + cruise_traj.points.at(i).longitudinal_velocity_mps = 0.0; + } + + return cruise_traj; } -void PIDBasedPlanner::publishDebugValues(const ObstacleCruisePlannerData & planner_data) const +Trajectory PIDBasedPlanner::getAccelerationLimitedTrajectory( + const Trajectory traj, const geometry_msgs::msg::Pose & start_pose, const double v0, + const double a0, const double target_acc, const double target_jerk_ratio) const { - const auto debug_values_msg = convertDebugValuesToMsg(planner_data.current_time, debug_values_); - debug_values_pub_->publish(debug_values_msg); + // calculate vt function + const auto & i = longitudinal_info_; + const auto vt = [&]( + const double v0, const double a0, const double jerk, const double t, + const double target_acc) { + const double t1 = (target_acc - a0) / jerk; + + const double v = [&]() { + if (t < t1) { + return v0 + a0 * t + jerk * t * t / 2.0; + } + + const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; + return v1 + target_acc * (t - t1); + }(); + + constexpr double max_vel = 100.0; + return std::clamp(v, 0.0, max_vel); + }; + + // calculate sv graph + const double s_traj = motion_utils::calcArcLength(traj.points); + // const double t_max = 10.0; + const double s_max = 50.0; + const double max_sampling_num = 100.0; + + const double t_delta_min = 0.1; + const double s_delta_min = 0.1; + + // NOTE: v0 of obstacle_cruise_planner may be smaller than v0 of motion_velocity_smoother + // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. + const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; + const double a1 = a0; // + jerk * 0.1; + const double jerk = + target_acc > a1 ? i.max_jerk * target_jerk_ratio : i.min_jerk * target_jerk_ratio; + + double t_current = 0.0; + std::vector s_vec{0.0}; + std::vector v_vec{v1}; + for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) { + if (v_vec.back() <= 0.0) { + s_vec.push_back(s_vec.back() + s_delta_min); + v_vec.push_back(0.0); + } else { + const double v_current = vt( + v1, a1, jerk, t_current + t_delta_min, + target_acc); // TODO(murooka) + t_delta_min is not required + + const double s_delta = std::max(s_delta_min, v_current * t_delta_min); + const double s_current = s_vec.back() + s_delta; + + s_vec.push_back(s_current); + v_vec.push_back(v_current); + + const double t_delta = [&]() { + if (v_current <= 0) { + return 0.0; + } + + constexpr double t_delta_max = 100.0; // NOTE: to avoid computation explosion + return std::min(t_delta_max, s_delta / v_current); + }(); + + t_current += t_delta; + } + + if (s_traj < s_vec.back() /*|| t_max < t_current*/ || s_max < s_vec.back()) { + break; + } + } + + std::vector unique_s_vec{s_vec.front()}; + std::vector unique_v_vec{v_vec.front()}; + for (size_t i = 0; i < s_vec.size(); ++i) { + if (s_vec.at(i) == unique_s_vec.back()) { + continue; + } + + unique_s_vec.push_back(s_vec.at(i)); + unique_v_vec.push_back(v_vec.at(i)); + } + + if (unique_s_vec.size() < 2) { + return traj; + } + + auto acc_limited_traj = traj; + const size_t ego_seg_idx = findEgoIndex(acc_limited_traj, start_pose); + double sum_dist = 0.0; + for (size_t i = ego_seg_idx; i < acc_limited_traj.points.size(); ++i) { + if (i != ego_seg_idx) { + sum_dist += tier4_autoware_utils::calcDistance2d( + acc_limited_traj.points.at(i - 1), acc_limited_traj.points.at(i)); + } + + const double vel = [&]() { + if (unique_s_vec.back() < sum_dist) { + return unique_v_vec.back(); + } + return interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + }(); + + acc_limited_traj.points.at(i).longitudinal_velocity_mps = std::clamp( + vel, 0.0, + static_cast( + acc_limited_traj.points.at(i) + .longitudinal_velocity_mps)); // TODO(murooka) this assumes forward driving + } + + return acc_limited_traj; } void PIDBasedPlanner::updateParam(const std::vector & parameters) @@ -260,24 +616,83 @@ void PIDBasedPlanner::updateParam(const std::vector & paramet tier4_autoware_utils::updateParam( parameters, "pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); - // pid controller - double kp = pid_controller_->getKp(); - double ki = pid_controller_->getKi(); - double kd = pid_controller_->getKd(); - - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kp", kp); - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.ki", ki); - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kd", kd); - tier4_autoware_utils::updateParam( - parameters, "pid_based_planner.output_ratio_during_accel", output_ratio_during_accel_); + { // velocity limit based planner + auto & p = velocity_limit_based_planner_param_; + + double kp = p.pid_vel_controller->getKp(); + double ki = p.pid_vel_controller->getKi(); + double kd = p.pid_vel_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.kp", kp); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.ki", ki); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.kd", kd); + p.pid_vel_controller->updateParam(kp, ki, kd); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", + p.output_ratio_during_accel); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", + p.disable_target_acceleration); + } - // vel_to_acc_weight - tier4_autoware_utils::updateParam( - parameters, "pid_based_planner.vel_to_acc_weight", vel_to_acc_weight_); + { // velocity insertion based planner + auto & p = velocity_insertion_based_planner_param_; + + // pid controller for acc + double kp_acc = p.pid_acc_controller->getKp(); + double ki_acc = p.pid_acc_controller->getKi(); + double kd_acc = p.pid_acc_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); + p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + double kp_jerk = p.pid_jerk_controller->getKp(); + double ki_jerk = p.pid_jerk_controller->getKi(); + double kd_jerk = p.pid_jerk_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); + p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); + + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", + p.output_acc_ratio_during_accel); + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", + p.output_jerk_ratio_during_accel); + + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } // min_cruise_target_vel tier4_autoware_utils::updateParam( parameters, "pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - - pid_controller_->updateParam(kp, ki, kd); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 45ed1d3f92d0e..420b72d2bf0aa 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -35,26 +35,25 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( header.stamp = current_time; // create stop factor - tier4_planning_msgs::msg::StopFactor stop_factor; + StopFactor stop_factor; stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_points.front().point; stop_factor_point.z = stop_pose.position.z; stop_factor.stop_factor_points.emplace_back(stop_factor_point); // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; stop_reason_msg.stop_factors.emplace_back(stop_factor); // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; } -tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( - const rclcpp::Time & current_time) +StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time) { // create header std_msgs::msg::Header header; @@ -62,11 +61,11 @@ tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( header.stamp = current_time; // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; @@ -106,6 +105,12 @@ double calcMinimumDistanceToStop( Trajectory PlannerInterface::generateStopTrajectory( const ObstacleCruisePlannerData & planner_data, DebugData & debug_data) { + stop_planning_debug_info_.reset(); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_vel); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_acc); + const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); @@ -139,8 +144,8 @@ Trajectory PlannerInterface::generateStopTrajectory( planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point); const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( - planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_data.traj.points, planner_data.current_pose, 0, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (!negative_dist_to_ego) { // delete marker const auto markers = @@ -155,9 +160,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // we set closest_obstacle_stop_distance to closest_behavior_stop_distance const double margin_from_obstacle = [&]() { const size_t nearest_segment_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + findEgoSegmentIndex(planner_data.traj, planner_data.current_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj.points, nearest_segment_idx + 1); @@ -236,6 +239,17 @@ Trajectory PlannerInterface::generateStopTrajectory( stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); } + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + closest_obstacle_dist - abs_ego_offset); // TODO(murooka) + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); + + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, feasible_margin_from_obstacle); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); + return output_traj; } @@ -248,7 +262,7 @@ double PlannerInterface::calcDistanceToCollisionPoint( const auto dist_to_collision_point = motion_utils::calcSignedArcLength( planner_data.traj.points, planner_data.current_pose, collision_point, - nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_collision_point) { return dist_to_collision_point.get() - offset; diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index d0b635951e074..28fa21646062a 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -88,9 +88,8 @@ Polygon2d createOneStepPolygon( namespace polygon_utils { boost::optional getCollisionIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & shape, + const Trajectory & traj, const std::vector & traj_polygons, + const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape, std::vector & collision_geom_points, const double max_dist) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose.pose, shape); @@ -128,10 +127,9 @@ boost::optional getCollisionIndex( } std::vector getCollisionPoints( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, std::vector & collision_index, const double max_dist, const double max_prediction_time_for_collision_check) @@ -172,11 +170,10 @@ std::vector getCollisionPoints( } std::vector willCollideWithSurroundObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, - const double max_dist, const double ego_obstacle_overlap_time_threshold, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double max_dist, + const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward) { @@ -200,8 +197,8 @@ std::vector willCollideWithSurroundObstacle( } std::vector createOneStepPolygons( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, + const double expand_width) { std::vector polygons; @@ -223,8 +220,8 @@ std::vector createOneStepPolygons( geometry_msgs::msg::PointStamped calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, - const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, - const double vehicle_max_longitudinal_offset, const bool is_driving_forward) + const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset, + const bool is_driving_forward) { std::vector segment_points(2); if (first_within_idx == 0) { diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 8aecbf393bcc0..45cc0a2e6c89d 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -18,12 +18,6 @@ namespace obstacle_cruise_utils { -bool isVehicle(const uint8_t label) -{ - return label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || - label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE; -} - visualization_msgs::msg::Marker getObjectMarker( const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, const double r, const double g, const double b) @@ -41,8 +35,7 @@ visualization_msgs::msg::Marker getObjectMarker( } boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, - const double target_length) + const Trajectory & traj, const size_t start_idx, const double target_length) { if (traj.points.empty()) { return {}; @@ -77,8 +70,8 @@ boost::optional calcForwardPose( } boost::optional getCurrentObjectPoseFromPredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) { const double rel_time = (current_time - obj_base_time).seconds(); if (rel_time < 0.0) { @@ -89,8 +82,8 @@ boost::optional getCurrentObjectPoseFromPredictedPath( } boost::optional getCurrentObjectPoseFromPredictedPaths( - const std::vector & predicted_paths, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) { if (predicted_paths.empty()) { return boost::none; @@ -98,19 +91,14 @@ boost::optional getCurrentObjectPoseFromPredictedPaths // Get the most reliable path const auto predicted_path = std::max_element( predicted_paths.begin(), predicted_paths.end(), - []( - const autoware_auto_perception_msgs::msg::PredictedPath & a, - const autoware_auto_perception_msgs::msg::PredictedPath & b) { - return a.confidence < b.confidence; - }); + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time); } geometry_msgs::msg::PoseStamped getCurrentObjectPose( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, - const std_msgs::msg::Header & obj_header, const rclcpp::Time & current_time, - const bool use_prediction) + const PredictedObject & predicted_object, const std_msgs::msg::Header & obj_header, + const rclcpp::Time & current_time, const bool use_prediction) { if (!use_prediction) { geometry_msgs::msg::PoseStamped current_pose; @@ -119,7 +107,7 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( return current_pose; } - std::vector predicted_paths; + std::vector predicted_paths; for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); } @@ -143,8 +131,7 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( } boost::optional getClosestStopObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & target_obstacles) + const Trajectory & traj, const std::vector & target_obstacles) { if (target_obstacles.empty()) { return boost::none; @@ -167,13 +154,4 @@ boost::optional getClosestStopObstacle( } return closest_stop_obstacle; } - -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} } // namespace obstacle_cruise_utils diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index e3e4c6d75d6ed..a70ab56d98a94 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -87,7 +87,7 @@ class TrajectoryAnalyzer TrajectoryDebugInfo data; data.stamp = node_->now(); data.size = points.size(); - data.curvature = calcCurvature(points); + data.curvature = motion_utils::calcCurvature(points); const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p); data.arclength = calcPathArcLengthArray(points, -arclength_offset); data.velocity = getVelocityArray(points); diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index e3b4ab7639cb6..96488fbde4b96 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -114,21 +114,6 @@ std::vector calcPathArcLengthArray(const T & points, const double offset return out; } -template -inline std::vector calcCurvature(const T & points) -{ - std::vector curvature_arr; - curvature_arr.push_back(0.0); - for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = getPoint(points.at(i - 1)); - const auto p2 = getPoint(points.at(i)); - const auto p3 = getPoint(points.at(i + 1)); - curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); - } - curvature_arr.push_back(0.0); - return curvature_arr; -} - } // namespace planning_debug_tools #endif // PLANNING_DEBUG_TOOLS__UTIL_HPP_ diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp index adb6e85e463a7..d36974508d2df 100644 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp @@ -18,18 +18,24 @@ #include "rclcpp/rclcpp.hpp" #include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" +#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp" + #include #include #include namespace rtc_auto_mode_manager { +using tier4_rtc_msgs::msg::AutoModeStatusArray; class RTCAutoModeManagerNode : public rclcpp::Node { public: explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options); private: + rclcpp::TimerBase::SharedPtr timer_; + AutoModeStatusArray auto_mode_statuses_; + rclcpp::Publisher::SharedPtr statuses_pub_; std::vector> managers_; }; diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp index 0a9b8add0e0ee..098852c397161 100644 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp @@ -17,6 +17,8 @@ #include "rclcpp/rclcpp.hpp" +#include "tier4_rtc_msgs/msg/auto_mode_status.hpp" +#include "tier4_rtc_msgs/msg/module.hpp" #include "tier4_rtc_msgs/srv/auto_mode.hpp" #include @@ -25,6 +27,8 @@ namespace rtc_auto_mode_manager { +using tier4_rtc_msgs::msg::AutoModeStatus; +using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::srv::AutoMode; class RTCAutoModeManagerInterface @@ -32,12 +36,13 @@ class RTCAutoModeManagerInterface public: RTCAutoModeManagerInterface( rclcpp::Node * node, const std::string & module_name, const bool default_enable); + AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; } private: void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const; + const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const; - + AutoModeStatus auto_mode_status_; rclcpp::Client::SharedPtr enable_cli_; rclcpp::Service::SharedPtr enable_srv_; diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp index 98a44f97b9ee3..a37987c2efe39 100644 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ b/planning/rtc_auto_mode_manager/src/node.cpp @@ -32,6 +32,17 @@ RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_ std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0; managers_.push_back(std::make_shared(this, module_name, enabled)); } + statuses_pub_ = create_publisher("output/auto_mode_statuses", 1); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] { + AutoModeStatusArray auto_mode_statuses; + for (const auto & m : managers_) { + auto_mode_statuses.stamp = get_clock()->now(); + auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus()); + } + statuses_pub_->publish(auto_mode_statuses); + auto_mode_statuses.statuses.clear(); + }); } } // namespace rtc_auto_mode_manager diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index 5cdc0f7dce38d..9ac306534e62e 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -16,6 +16,43 @@ namespace rtc_auto_mode_manager { +Module getModuleType(const std::string & module_name) +{ + Module module; + if (module_name == "blind_spot") { + module.type = Module::BLIND_SPOT; + } else if (module_name == "crosswalk") { + module.type = Module::CROSSWALK; + } else if (module_name == "detection_area") { + module.type = Module::DETECTION_AREA; + } else if (module_name == "intersection") { + module.type = Module::INTERSECTION; + } else if (module_name == "no_stopping_area") { + module.type = Module::NO_STOPPING_AREA; + } else if (module_name == "occlusion_spot") { + module.type = Module::OCCLUSION_SPOT; + } else if (module_name == "traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "virtual_traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "lane_change_left") { + module.type = Module::LANE_CHANGE_LEFT; + } else if (module_name == "lane_change_right") { + module.type = Module::LANE_CHANGE_RIGHT; + } else if (module_name == "avoidance_left") { + module.type = Module::AVOIDANCE_LEFT; + } else if (module_name == "avoidance_right") { + module.type = Module::AVOIDANCE_RIGHT; + } else if (module_name == "pull_over") { + module.type = Module::PULL_OVER; + } else if (module_name == "pull_out") { + module.type = Module::PULL_OUT; + } else { + module.type = Module::NONE; + } + return module; +} + RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( rclcpp::Node * node, const std::string & module_name, const bool default_enable) { @@ -41,8 +78,10 @@ RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( enable_auto_mode_namespace_ + "/" + module_name, std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2)); + auto_mode_status_.module = getModuleType(module_name); // Send enable auto mode request if (default_enable) { + auto_mode_status_.is_auto_mode = true; AutoMode::Request::SharedPtr request = std::make_shared(); request->enable = true; enable_cli_->async_send_request(request); @@ -50,8 +89,9 @@ RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( } void RTCAutoModeManagerInterface::onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const + const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) { + auto_mode_status_.is_auto_mode = request->enable; enable_cli_->async_send_request(request); response->success = true; } diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_optimizer/CMakeLists.txt index 16d1915198657..73bb97761ba67 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -54,6 +54,17 @@ ament_auto_package( rviz ) +if(BUILD_TESTING) + add_launch_test( + test/test_static_centerline_optimizer.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) +endif() + install(PROGRAMS scripts/app.py DESTINATION lib/${PROJECT_NAME} diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml b/planning/static_centerline_optimizer/config/common.param.yaml similarity index 100% rename from launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml rename to planning/static_centerline_optimizer/config/common.param.yaml diff --git a/launch/tier4_control_launch/config/common/nearest_search.param.yaml b/planning/static_centerline_optimizer/config/nearest_search.param.yaml similarity index 100% rename from launch/tier4_control_launch/config/common/nearest_search.param.yaml rename to planning/static_centerline_optimizer/config/nearest_search.param.yaml diff --git a/planning/static_centerline_optimizer/config/vehicle_info.param.yaml b/planning/static_centerline_optimizer/config/vehicle_info.param.yaml new file mode 100644 index 0000000000000..8941b92b4e78e --- /dev/null +++ b/planning/static_centerline_optimizer/config/vehicle_info.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + wheel_radius: 0.39 + wheel_width: 0.42 + wheel_base: 2.74 # between front wheel center and rear wheel center + wheel_tread: 1.63 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.03 # between rear wheel center and vehicle rear + left_overhang: 0.1 # between left wheel center and vehicle left + right_overhang: 0.1 # between right wheel center and vehicle right + vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index d50c30fdd12d5..0cdd6b58c74a9 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -5,6 +5,7 @@ 0.1.0 The static_centerline_optimizer package Takayuki Murooka + Kosuke Takeuchi Apache License 2.0 Takayuki Murooka @@ -33,10 +34,13 @@ tier4_autoware_utils vehicle_info_util + python3-flask-cors rosidl_default_runtime + ament_cmake_gmock ament_lint_auto autoware_lint_common + ros_testing rosidl_interface_packages diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz index 6ce4410249f39..30a3776fd8526 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz +++ b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz @@ -177,27 +177,6 @@ Visualization Manager: Offset: 0 Radius: 0.10000000149011612 Value: false - - Alpha: 0.3499999940395355 - Class: rviz_plugins/DrivableArea - Color Scheme: map - Draw Behind: true - Enabled: true - Name: DrivableArea - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_optimizer/debug/raw_centerline - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_optimizer/debug/raw_centerline_updates - Use Timestamp: false - Value: true - Class: rviz_common/Group Displays: - Class: rviz_plugins/PathFootprint diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index f97d08b113e2a..3999c5cbce50a 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -85,9 +85,6 @@ PathWithLaneId get_path_with_lane_id( auto planner_data = std::make_shared(); planner_data->route_handler = std::make_shared(route_handler); planner_data->self_pose = convert_to_pose_stamped(start_pose); - planner_data->parameters.drivable_lane_forward_length = std::numeric_limits::max(); - planner_data->parameters.drivable_lane_backward_length = std::numeric_limits::min(); - planner_data->parameters.drivable_lane_margin = 5.0; planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; diff --git a/planning/static_centerline_optimizer/test/data/lanelet2_map.osm b/planning/static_centerline_optimizer/test/data/lanelet2_map.osm new file mode 100644 index 0000000000000..406cd85c151ea --- /dev/null +++ b/planning/static_centerline_optimizer/test/data/lanelet2_map.osm @@ -0,0 +1,146 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py new file mode 100644 index 0000000000000..517b6eaf9e477 --- /dev/null +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + + lanelet2_map_path = os.path.join( + get_package_share_directory("static_centerline_optimizer"), "test/data/lanelet2_map.osm" + ) + + static_centerline_optimizer_node = Node( + package="static_centerline_optimizer", + executable="main", + output="screen", + parameters=[ + {"lanelet2_map_path": lanelet2_map_path}, + {"run_background": False}, + {"rviz": False}, + {"lanelet2_input_file_path": lanelet2_map_path}, + {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, + {"start_lanelet_id": 215}, + {"end_lanelet_id": 216}, + os.path.join( + get_package_share_directory("static_centerline_optimizer"), + "config/static_centerline_optimizer.param.yaml", + ), + os.path.join( + get_package_share_directory("obstacle_avoidance_planner"), + "config/obstacle_avoidance_planner.param.yaml", + ), + os.path.join( + get_package_share_directory("map_loader"), + "config/lanelet2_map_loader.param.yaml", + ), + os.path.join( + get_package_share_directory("static_centerline_optimizer"), + "config/common.param.yaml", + ), + os.path.join( + get_package_share_directory("static_centerline_optimizer"), + "config/nearest_search.param.yaml", + ), + os.path.join( + get_package_share_directory("static_centerline_optimizer"), + "config/vehicle_info.param.yaml", + ), + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + static_centerline_optimizer_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 74a46b5fe0951..d8d561a9c5098 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) -find_package(OpenMP) find_package(CGAL REQUIRED COMPONENTS Core) include_directories( @@ -21,7 +20,6 @@ include_directories( ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} - ${GRID_MAP_INCLUDE_DIR} ) add_library(pointcloud_preprocessor_filter_base SHARED @@ -72,13 +70,6 @@ target_link_libraries(pointcloud_preprocessor_filter ${PCL_LIBRARIES} ) -if(OPENMP_FOUND) - set_target_properties(pointcloud_preprocessor_filter PROPERTIES - COMPILE_FLAGS ${OpenMP_CXX_FLAGS} - LINK_FLAGS ${OpenMP_CXX_FLAGS} - ) -endif() - # ========== Concatenate data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" 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 d7d86c75364b8..121d9511478a7 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 @@ -125,7 +125,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt create_publisher("output/turn_indicators_report", QoS{1}); pub_hazard_lights_report_ = create_publisher("output/hazard_lights_report", QoS{1}); - pub_current_pose_ = create_publisher("/current_pose", QoS{1}); + pub_current_pose_ = create_publisher("output/debug/pose", QoS{1}); pub_velocity_ = create_publisher("output/twist", QoS{1}); pub_odom_ = create_publisher("output/odometry", QoS{1}); pub_steer_ = create_publisher("output/steering", QoS{1}); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 503f7964a6413..e15de4152290d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -109,6 +109,14 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( void SimModelDelaySteerAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { + const auto setStopState = [&]() { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + }; + using autoware_auto_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || @@ -120,31 +128,15 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { if (state(IDX::VX) < 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { if (state(IDX::VX) > 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::PARK) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } else { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 345fec56233a6..8abad2df84936 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -64,6 +64,13 @@ Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel( void SimModelIdealSteerAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { + const auto setStopState = [&]() { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + }; + using autoware_auto_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || @@ -75,31 +82,17 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { if (state(IDX::VX) < 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { if (state(IDX::VX) > 0.0) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } } else if (gear == GearCommand::PARK) { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } else { - state(IDX::VX) = 0.0; - state(IDX::X) = prev_state(IDX::X); - state(IDX::Y) = prev_state(IDX::Y); - state(IDX::YAW) = prev_state(IDX::YAW); - current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + setStopState(); } + // calculate acc from velocity diff + current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); } diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 9f56c8bffec96..5a5f2e880bcae 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -82,7 +82,7 @@ args: node_name_suffix: mission_planning_route topic: /planning/mission_planning/route - topic_type: autoware_auto_planning_msgs/msg/LaneletRoute + topic_type: autoware_planning_msgs/msg/LaneletRoute best_effort: false transient_local: true warn_rate: 0.0 diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index e0656a15b82f3..6221a370eab72 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/localization.cpp src/motion.cpp src/operation_mode.cpp + src/planning.cpp src/routing.cpp src/utils/route_conversion.cpp src/compatibility/autoware_state.cpp @@ -22,6 +23,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" "default_ad_api::OperationModeNode" + "default_ad_api::PlanningNode" "default_ad_api::RoutingNode" ) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index d3182a9070c2a..50211ede05e86 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -47,6 +47,7 @@ def generate_launch_description(): create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), + create_api_node("planning", "PlanningNode"), create_api_node("routing", "RoutingNode"), ] container = ComposableNodeContainer( diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp new file mode 100644 index 0000000000000..2fa7ae5b97749 --- /dev/null +++ b/system/default_ad_api/src/planning.cpp @@ -0,0 +1,151 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planning.hpp" + +#include +#include +#include + +namespace default_ad_api +{ + +template +void concat(std::vector & v1, const std::vector & v2) +{ + v1.insert(v1.end(), v2.begin(), v2.end()); +} + +template +std::vector::SharedPtr> init_factors( + rclcpp::Node * node, std::vector & factors, + const std::vector & topics) +{ + const auto callback = [&factors](const int index) { + return [&factors, index](const typename T::ConstSharedPtr msg) { factors[index] = msg; }; + }; + + std::vector::SharedPtr> subs; + for (size_t index = 0; index < topics.size(); ++index) { + subs.push_back(node->create_subscription(topics[index], rclcpp::QoS(1), callback(index))); + } + factors.resize(topics.size()); + return subs; +} + +template +T merge_factors(const rclcpp::Time stamp, const std::vector & factors) +{ + T message; + message.header.stamp = stamp; + message.header.frame_id = "map"; + + for (const auto & factor : factors) { + if (factor) { + concat(message.factors, factor->factors); + } + } + return message; +} + +PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning", options) +{ + // TODO(Takagi, Isamu): remove default value + stop_distance_ = declare_parameter("stop_distance", 1.0); + stop_duration_ = declare_parameter("stop_duration", 1.0); + stop_checker_ = std::make_unique(this, stop_duration_ + 1.0); + + std::vector velocity_factor_topics = { + "/planning/velocity_factors/blind_spot", + "/planning/velocity_factors/crosswalk", + "/planning/velocity_factors/detection_area", + "/planning/velocity_factors/intersection", + "/planning/velocity_factors/merge_from_private", + "/planning/velocity_factors/no_stopping_area", + "/planning/velocity_factors/obstacle_stop", + "/planning/velocity_factors/obstacle_cruise", + "/planning/velocity_factors/occlusion_spot", + "/planning/velocity_factors/stop_line", + "/planning/velocity_factors/surround_obstacle", + "/planning/velocity_factors/traffic_light", + "/planning/velocity_factors/virtual_traffic_light", + "/planning/velocity_factors/walkway"}; + + std::vector steering_factor_topics = { + "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", + "/planning/steering_factor/lane_change", "/planning/steering_factor/pull_out", + "/planning/steering_factor/pull_over"}; + + sub_velocity_factors_ = + init_factors(this, velocity_factors_, velocity_factor_topics); + sub_steering_factors_ = + init_factors(this, steering_factors_, steering_factor_topics); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_velocity_factors_); + adaptor.init_pub(pub_steering_factors_); + adaptor.init_sub(sub_kinematic_state_, this, &PlanningNode::on_kinematic_state); + adaptor.init_sub(sub_trajectory_, this, &PlanningNode::on_trajectory); + + const auto rate = rclcpp::Rate(5); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); +} + +void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } + +void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) +{ + kinematic_state_ = msg; + + geometry_msgs::msg::TwistStamped twist; + twist.header = msg->header; + twist.twist = msg->twist.twist; + stop_checker_->addTwist(twist); +} + +void PlanningNode::on_timer() +{ + using autoware_adapi_v1_msgs::msg::VelocityFactor; + auto velocity = merge_factors(now(), velocity_factors_); + auto steering = merge_factors(now(), steering_factors_); + + // Set the distance if it is nan. + if (trajectory_ && kinematic_state_) { + for (auto & factor : velocity.factors) { + if (std::isnan(factor.distance)) { + const auto & curr_point = kinematic_state_->pose.pose.position; + const auto & stop_point = factor.pose.position; + const auto & points = trajectory_->points; + factor.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + } + } + } + + // Set the status if it is unknown. + const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(stop_duration_); + for (auto & factor : velocity.factors) { + if ((factor.status == VelocityFactor::UNKNOWN) && (!std::isnan(factor.distance))) { + const auto is_stopped = is_vehicle_stopped && (factor.distance < stop_distance_); + factor.status = is_stopped ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING; + } + } + + pub_velocity_factors_->publish(velocity); + pub_steering_factors_->publish(steering); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PlanningNode) diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp new file mode 100644 index 0000000000000..03b45fd3b5d41 --- /dev/null +++ b/system/default_ad_api/src/planning.hpp @@ -0,0 +1,67 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PLANNING_HPP_ +#define PLANNING_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class PlanningNode : public rclcpp::Node +{ +public: + explicit PlanningNode(const rclcpp::NodeOptions & options); + +private: + using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; + using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; + Pub pub_velocity_factors_; + Pub pub_steering_factors_; + Sub sub_trajectory_; + Sub sub_kinematic_state_; + std::vector::SharedPtr> sub_velocity_factors_; + std::vector::SharedPtr> sub_steering_factors_; + std::vector velocity_factors_; + std::vector steering_factors_; + rclcpp::TimerBase::SharedPtr timer_; + + using VehicleStopChecker = motion_utils::VehicleStopCheckerBase; + using Trajectory = planning_interface::Trajectory::Message; + using KinematicState = localization_interface::KinematicState::Message; + void on_trajectory(const Trajectory::ConstSharedPtr msg); + void on_kinematic_state(const KinematicState::ConstSharedPtr msg); + void on_timer(); + + double stop_distance_; + double stop_duration_; + std::unique_ptr stop_checker_; + Trajectory::ConstSharedPtr trajectory_; + KinematicState::ConstSharedPtr kinematic_state_; +}; + +} // namespace default_ad_api + +#endif // PLANNING_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/__init__.py b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/__init__.py new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py new file mode 100755 index 0000000000000..194551d56dcd5 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +from autoware_adapi_v1_msgs.msg import SteeringFactor +from autoware_adapi_v1_msgs.msg import SteeringFactorArray +from autoware_adapi_v1_msgs.msg import VelocityFactor +from autoware_adapi_v1_msgs.msg import VelocityFactorArray +from geometry_msgs.msg import Pose +import numpy +import rclpy +import rclpy.duration +import rclpy.node +import tf_transformations +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + +velocity_type_text = { + VelocityFactor.UNKNOWN: "unknown", + VelocityFactor.SURROUNDING_OBSTACLE: "surrounding obstacle", + VelocityFactor.ROUTE_OBSTACLE: "route obstacle", + VelocityFactor.INTERSECTION: "intersection", + VelocityFactor.CROSSWALK: "crosswalk", + VelocityFactor.REAR_CHECK: "rear check", + VelocityFactor.USER_DEFINED_DETECTION_AREA: "detection area", + VelocityFactor.NO_STOPPING_AREA: "no stopping area", + VelocityFactor.STOP_SIGN: "stop sign", + VelocityFactor.TRAFFIC_SIGNAL: "traffic signal", + VelocityFactor.V2I_GATE_CONTROL_ENTER: "v2i enter", + VelocityFactor.V2I_GATE_CONTROL_LEAVE: "v2i leave", + VelocityFactor.MERGE: "merge", + VelocityFactor.SIDEWALK: "sidewalk", + VelocityFactor.LANE_CHANGE: "lane change", + VelocityFactor.AVOIDANCE: "avoidance", + VelocityFactor.EMERGENCY_STOP_OPERATION: "emergency stop operation", +} + +steering_type_text = { + SteeringFactor.INTERSECTION: "intersection", + SteeringFactor.LANE_CHANGE: "lane change", + SteeringFactor.AVOIDANCE_PATH_CHANGE: "avoidance change", + SteeringFactor.AVOIDANCE_PATH_RETURN: "avoidance return", + SteeringFactor.STATION: "station", + SteeringFactor.PULL_OUT: "pull out", + SteeringFactor.PULL_OVER: "pull over", + SteeringFactor.EMERGENCY_OPERATION: "emergency operation", +} + +velocity_status_color = { + VelocityFactor.UNKNOWN: (1.0, 1.0, 1.0), + VelocityFactor.APPROACHING: (1.0, 0.0, 0.0), + VelocityFactor.STOPPED: (1.0, 1.0, 0.0), +} + +steering_status_color = { + SteeringFactor.UNKNOWN: (1.0, 1.0, 1.0), + SteeringFactor.APPROACHING: (1.0, 0.0, 0.0), + SteeringFactor.TRYING: (1.0, 1.0, 0.0), + SteeringFactor.TURNING: (1.0, 1.0, 1.0), +} + + +class PlanningFactorVisualizer(rclpy.node.Node): + def __init__(self): + super().__init__("planning_factor_visualizer") + self.front_offset = self.declare_parameter("front_offset", 0.0).value + self.pub_velocity = self.create_publisher(MarkerArray, "/visualizer/velocity_factors", 1) + self.pub_steering = self.create_publisher(MarkerArray, "/visualizer/steering_factors", 1) + self.sub_velocity = self.create_subscription(VelocityFactorArray, "/api/planning/velocity_factors", self.on_velocity_factor, 1) # fmt: skip + self.sub_steering = self.create_subscription(SteeringFactorArray, "/api/planning/steering_factors", self.on_steering_factor, 1) # fmt: skip + + def on_velocity_factor(self, msg): + markers = MarkerArray() + for index, factor in enumerate(msg.factors): + color = velocity_status_color[factor.status] + text = velocity_type_text[factor.type] + markers.markers.append(self.create_wall(index, msg.header, factor.pose, color)) + markers.markers.append(self.create_text(index, msg.header, factor.pose, text)) + self.pub_velocity.publish(markers) + + def on_steering_factor(self, msg): + markers = MarkerArray() + for index, factor in enumerate(msg.factors): + index1 = index * 2 + 0 + index2 = index * 2 + 1 + color = steering_status_color[factor.status] + text = steering_type_text[factor.type] + markers.markers.append(self.create_wall(index1, msg.header, factor.pose[0], color)) + markers.markers.append(self.create_wall(index2, msg.header, factor.pose[1], color)) + markers.markers.append(self.create_text(index1, msg.header, factor.pose[0], text)) + markers.markers.append(self.create_text(index2, msg.header, factor.pose[1], text)) + self.pub_steering.publish(markers) + + def offset_pose(self, pose): + q = pose.orientation + q = numpy.array([q.x, q.y, q.z, q.w]) + p = numpy.array([self.front_offset, 0, 0, 1]) + r = tf_transformations.quaternion_matrix(q) + x = numpy.dot(r, p) + result = Pose() + result.position.x = pose.position.x + x[0] + result.position.y = pose.position.y + x[1] + result.position.z = pose.position.z + x[2] + result.orientation = pose.orientation + return result + + def create_wall(self, index, header, pose, color): + pose = self.offset_pose(pose) + marker = Marker() + marker.ns = "wall" + marker.id = index + marker.lifetime = rclpy.duration.Duration(nanoseconds=0.3 * 1e9).to_msg() + marker.header = header + marker.action = Marker.ADD + marker.type = Marker.CUBE + marker.pose.position.x = pose.position.x + marker.pose.position.y = pose.position.y + marker.pose.position.z = pose.position.z + 1.0 + marker.pose.orientation = pose.orientation + marker.scale.x = 0.1 + marker.scale.y = 5.0 + marker.scale.z = 2.0 + marker.color.a = 0.7 + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + return marker + + def create_text(self, index, header, pose, text): + pose = self.offset_pose(pose) + marker = Marker() + marker.ns = "type" + marker.id = index + marker.lifetime = rclpy.duration.Duration(nanoseconds=0.3 * 1e9).to_msg() + marker.header = header + marker.action = Marker.ADD + marker.type = Marker.TEXT_VIEW_FACING + marker.text = text + marker.pose.position.x = pose.position.x + marker.pose.position.y = pose.position.y + marker.pose.position.z = pose.position.z + 2.0 + marker.pose.orientation = pose.orientation + marker.scale.z = 1.0 + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 1.0 + return marker + + +def main(args=None): + try: + rclpy.init(args=args) + rclpy.spin(PlanningFactorVisualizer()) + rclpy.shutdown() + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml b/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml new file mode 100644 index 0000000000000..26fb4720ca435 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/system/default_ad_api_helpers/ad_api_visualizers/package.xml b/system/default_ad_api_helpers/ad_api_visualizers/package.xml new file mode 100644 index 0000000000000..642e07d724710 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/package.xml @@ -0,0 +1,25 @@ + + + + ad_api_visualizers + 0.1.0 + The ad_api_visualizers package + Takagi, Isamu + yabuta + Kah Hooi Tan + Apache License 2.0 + + autoware_adapi_v1_msgs + geometry_msgs + rclpy + tf_transformations + visualization_msgs + + ament_copyright + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/system/default_ad_api_helpers/ad_api_visualizers/resource/ad_api_visualizers b/system/default_ad_api_helpers/ad_api_visualizers/resource/ad_api_visualizers new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg b/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg new file mode 100644 index 0000000000000..b0af17360079d --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ad_api_visualizers +[install] +install_scripts=$base/lib/ad_api_visualizers diff --git a/system/default_ad_api_helpers/ad_api_visualizers/setup.py b/system/default_ad_api_helpers/ad_api_visualizers/setup.py new file mode 100644 index 0000000000000..5712980f8e3fb --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/setup.py @@ -0,0 +1,31 @@ +from warnings import simplefilter + +from pkg_resources import PkgResourcesDeprecationWarning +from setuptools import SetuptoolsDeprecationWarning +from setuptools import setup + +simplefilter("ignore", category=SetuptoolsDeprecationWarning) +simplefilter("ignore", category=PkgResourcesDeprecationWarning) + +package_name = "ad_api_visualizers" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", [f"launch/{package_name}.launch.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Takagi, Isamu", + maintainer_email="isamu.takagi@tier4.jp", + description="The ad_api_visualizers package", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["planning_factors = ad_api_visualizers.planning_factors:main"], + }, +) diff --git a/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py b/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py new file mode 100644 index 0000000000000..95f03810541d2 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py b/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py new file mode 100644 index 0000000000000..a2c3deb8eb3de --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 01e973b8ff26a..87cf767accc06 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -20,6 +20,12 @@ contains: [": emergency_stop_operation"] timeout: 1.0 + service_log_checker: + type: diagnostic_aggregator/GenericAnalyzer + path: service_log_checker + contains: ["service_log_checker"] + timeout: 5.0 + resource_monitoring: type: diagnostic_aggregator/AnalyzerGroup path: resource_monitoring diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index b9de5cc4f2e13..71dc2ac600685 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -36,6 +36,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default @@ -46,5 +47,6 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index bf40c334f6498..9708456df4fe7 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -36,6 +36,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default @@ -46,5 +47,6 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default + /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_monitor/CMakeLists.txt b/system/system_monitor/CMakeLists.txt index 97eddcb590a5f..391c82a09c960 100644 --- a/system/system_monitor/CMakeLists.txt +++ b/system/system_monitor/CMakeLists.txt @@ -118,7 +118,8 @@ ament_auto_add_executable(hdd_reader ) ament_auto_add_executable(traffic_reader - reader/traffic_reader/traffic_reader.cpp + reader/traffic_reader/traffic_reader_main.cpp + reader/traffic_reader/traffic_reader_service.cpp ) find_library(NL3 nl-3 REQUIRED) diff --git a/system/system_monitor/README.md b/system/system_monitor/README.md index 87107d3937a82..6203ad45d4b3a 100644 --- a/system/system_monitor/README.md +++ b/system/system_monitor/README.md @@ -73,7 +73,8 @@ Every topic is published in 1 minute interval. | | HDD WriteIOPS | ✓ | ✓ | ✓ | | | | HDD Connection | ✓ | ✓ | ✓ | | | Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | -| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | +| Net Monitor | Network Connection | ✓ | ✓ | ✓ | | +| | Network Usage | ✓ | ✓ | ✓ | Notification of usage only, normally error not generated. | | | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | | | IP Packet Reassembles Failed | ✓ | ✓ | ✓ | | | NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/system_monitor/config/net_monitor.param.yaml index cb7e1b48380a3..f2c68adda2efe 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/system_monitor/config/net_monitor.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: devices: ["*"] - traffic_reader_port: 7636 monitor_program: "greengrass" crc_error_check_duration: 1 crc_error_count_threshold: 1 diff --git a/system/system_monitor/docs/ros_parameters.md b/system/system_monitor/docs/ros_parameters.md index bba2c3fb40408..1d081d513c85c 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/system_monitor/docs/ros_parameters.md @@ -61,14 +61,14 @@ mem_monitor: net_monitor: -| Name | Type | Unit | Default | Notes | -| :-------------------------------- | :----------: | :-----: | :-----: | :--------------------------------------------------------------------------------------------------------------------------------------------------- | -| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | -| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | -| crc_error_check_duration | int | sec | 1 | CRC error check duration. | -| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | -| reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | -| reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | +| Name | Type | Unit | Default | Notes | +| :-------------------------------- | :----------: | :--: | :--------: | :--------------------------------------------------------------------------------------------------------------------------------------------------- | +| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | +| monitor_program | string | n/a | greengrass | program name to be monitored by nethogs name. | +| crc_error_check_duration | int | sec | 1 | CRC error check duration. | +| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | +| reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | +| reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/system_monitor/docs/topics_net_monitor.md index d223b359cdb08..edb067c0a5be7 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/system_monitor/docs/topics_net_monitor.md @@ -1,5 +1,23 @@ # ROS topics: Net Monitor +## Network Connection + +/diagnostics/net_monitor: Network Connection + +[summary] + +| level | message | +| ----- | -------------- | +| OK | OK | +| WARN | no such device | + +[values] + +| key | value (example) | +| --------------------- | ------------------- | +| Network [0-9]: status | OK / no such device | +| HDD [0-9]: name | wlp82s0 | + ## Network Usage /diagnostics/net_monitor: Network Usage @@ -38,31 +56,19 @@ | ----- | ------- | | OK | OK | -[values] program +[values when specified program is detected] | key | value (example) | | -------------------------------- | ------------------------------------------- | -| nethogs [0-9]: PROGRAM | /lambda/greengrassSystemComponents/1384/999 | -| nethogs [0-9]: SENT (KB/Sec) | 1.13574 | -| nethogs [0-9]: RECEIVED (KB/Sec) | 0.261914 | - -[values] all - -| key | value (example) | -| --------------------- | -------------------------------------------------------------- | -| nethogs: all (KB/Sec) | python3.7/1520/999 0.274414 0.354883 | -| | /lambda/greengrassSystemComponents/1299/999 0.487305 0.0966797 | -| | sshd: muser@pts/5/15917/1002 0.396094 0.0585938 | -| | /usr/bin/python3.7/2371/999 0 0 | -| | /greengrass/ggc/packages/1.10.0/bin/daemon/906/0 0 0 | -| | python3.7/4362/999 0 0 | -| | unknown TCP/0/0 0 0 | +| nethogs [0-9]: program | /lambda/greengrassSystemComponents/1384/999 | +| nethogs [0-9]: sent (KB/Sec) | 1.13574 | +| nethogs [0-9]: received (KB/Sec) | 0.261914 | -[values] error +[values when error is occurring] -| key | value (example) | -| ----- | ----------------------------------------------------- | -| error | [nethogs -t] execve failed: No such file or directory | +| key | value (example) | +| ----- | ---------------------------------------- | +| error | execve failed: No such file or directory | ## Network CRC Error @@ -70,9 +76,10 @@ [summary] -| level | message | -| ----- | ------- | -| OK | OK | +| level | message | +| ----- | --------- | +| OK | OK | +| WARN | CRC error | [values] diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 3ddb078fa00e1..ded9c398c6cd1 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -21,207 +21,290 @@ #define SYSTEM_MONITOR__NET_MONITOR__NET_MONITOR_HPP_ #include "system_monitor/net_monitor/nl80211.hpp" +#include "traffic_reader/traffic_reader_common.hpp" #include +#include + #include #include #include +#include #include #include -#define toMbit(X) (static_cast(X) / 1000000 * 8) +template +constexpr auto to_mbit(T value) +{ + return (static_cast(value) / 1000000 * 8); +} + +/** + * @brief Network information + */ +struct NetworkInfomation +{ + bool is_invalid; //!< @brief Valid network to be checked + int mtu_error_code; //!< @brief Error code set by ioctl() with SIOCGIFMTU + int ethtool_error_code; //!< @brief Error code set by ioctl() with SIOCETHTOOL + bool is_running; //!< @brief Resource allocated flag + std::string interface_name; //!< @brief Interface name + double speed; //!< @brief Network capacity + int mtu; //!< @brief MTU + double rx_traffic; //!< @brief Traffic received + double tx_traffic; //!< @brief Traffic transmitted + double rx_usage; //!< @brief Network capacity usage rate received + double tx_usage; //!< @brief Network capacity usage rate transmitted + unsigned int rx_bytes; //!< @brief Total bytes received + unsigned int rx_errors; //!< @brief Bad packets received + unsigned int tx_bytes; //!< @brief Total bytes transmitted + unsigned int tx_errors; //!< @brief Packet transmit problems + unsigned int collisions; //!< @brief Number of collisions during packet transmissions +}; /** * @brief Bytes information */ -typedef struct bytes +struct Bytes { - unsigned int rx_bytes; //!< @brief total bytes received - unsigned int tx_bytes; //!< @brief total bytes transmitted + unsigned int rx_bytes; //!< @brief Total bytes received + unsigned int tx_bytes; //!< @brief Total bytes transmitted +}; - bytes() : rx_bytes(0), tx_bytes(0) {} -} bytes; +/** + * @brief CRC errors information + */ +struct CrcErrors +{ + std::deque errors_queue{}; //!< @brief queue that holds count of CRC errors + unsigned int last_rx_crc_errors{0}; //!< @brief rx_crc_error at the time of the last monitoring +}; + +namespace local = boost::asio::local; class NetMonitor : public rclcpp::Node { public: /** - * @brief constructor + * @brief Constructor * @param [in] options Options associated with this node. */ explicit NetMonitor(const rclcpp::NodeOptions & options); + /** - * @brief destructor + * @brief Destructor */ - ~NetMonitor(); + ~NetMonitor() override; /** - * @brief Shutdown nl80211 object + * @brief Copy constructor */ - void shutdown_nl80211(); + NetMonitor(const NetMonitor &) = delete; + + /** + * @brief Copy assignment operator + */ + NetMonitor & operator=(const NetMonitor &) = delete; + + /** + * @brief Move constructor + */ + NetMonitor(const NetMonitor &&) = delete; + + /** + * @brief Move assignment operator + */ + NetMonitor & operator=(const NetMonitor &&) = delete; protected: using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; /** - * @brief check CPU usage - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Check network connection + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void check_connection(diagnostic_updater::DiagnosticStatusWrapper & status); + + /** + * @brief Check network usage + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void check_usage(diagnostic_updater::DiagnosticStatusWrapper & status); + + /** + * @brief Monitor network traffic + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void monitor_traffic(diagnostic_updater::DiagnosticStatusWrapper & status); + + /** + * @brief Check CRC error + * @param [out] status diagnostic message passed directly to diagnostic publish calls */ - void checkUsage( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + void check_crc_error(diagnostic_updater::DiagnosticStatusWrapper & status); /** - * @brief monitor traffic - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Check IP packet reassembles failed + * @param [out] status diagnostic message passed directly to diagnostic publish calls */ - void monitorTraffic( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + void check_reassembles_failed(diagnostic_updater::DiagnosticStatusWrapper & status); /** - * @brief check CRC error - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Timer callback */ - void checkCrcError( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + void on_timer(); /** - * @brief check IP packet reassembles failed - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Determine if it is a supported network + * @param [in] network Network infomation + * @param [in] index Index of network infomation index + * @param [out] status Diagnostic message passed directly to diagnostic publish calls + * @param [out] error_message Error message */ - void checkReassemblesFailed( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + static void make_invalid_diagnostic_status( + const NetworkInfomation & network, int index, + diagnostic_updater::DiagnosticStatusWrapper & status, std::string & error_message); /** - * @brief get wireless speed - * @param [in] ifa_name interface name - * @return wireless speed + * @brief Update list of network information */ - float getWirelessSpeed(const char * ifa_name); + void update_network_list(); /** - * @brief timer callback + * @brief Update network information by using socket + * @param [out] network Network information */ - void onTimer(); + void update_network_information_by_socket(NetworkInfomation & network); /** - * @brief update Network information list + * @brief Update network information about MTU + * @param [out] network Network information + * @param [in] socket File descriptor to socket */ - void updateNetworkInfoList(); + static void update_mtu(NetworkInfomation & network, int socket); /** - * @brief check NetMonitor General Infomation - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @return check result + * @brief Update network information about network capacity + * @param [out] network Network information + * @param [in] socket File descriptor to socket */ - bool checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat); + void update_network_capacity(NetworkInfomation & network, int socket); /** - * @brief Network information + * @brief Update network information by using routing netlink stats + * @param [out] network Network information + * @param [in] data Pointer to routing netlink stats + * @param [in] duration Time from previous measurement */ - struct NetworkInfo - { - int mtu_errno; //!< @brief errno set by ioctl() with SIOCGIFMTU - int ethtool_errno; //!< @brief errno set by ioctl() with SIOCETHTOOL - bool is_running; //!< @brief resource allocated flag - std::string interface_name; //!< @brief interface name - float speed; //!< @brief network capacity - int mtu; //!< @brief MTU - float rx_traffic; //!< @brief traffic received - float tx_traffic; //!< @brief traffic transmitted - float rx_usage; //!< @brief network capacity usage rate received - float tx_usage; //!< @brief network capacity usage rate transmitted - unsigned int rx_bytes; //!< @brief total bytes received - unsigned int rx_errors; //!< @brief bad packets received - unsigned int tx_bytes; //!< @brief total bytes transmitted - unsigned int tx_errors; //!< @brief packet transmit problems - unsigned int collisions; //!< @brief number of collisions during packet transmissions + void update_network_information_by_routing_netlink( + NetworkInfomation & network, void * data, const rclcpp::Duration & duration); - NetworkInfo() - : mtu_errno(0), - ethtool_errno(0), - is_running(false), - interface_name(""), - speed(0.0), - mtu(0), - rx_traffic(0.0), - tx_traffic(0.0), - rx_usage(0.0), - tx_usage(0.0), - rx_bytes(0), - rx_errors(0), - tx_bytes(0), - tx_errors(0), - collisions(0) - { - } - }; + /** + * @brief Update network information about network traffic + * @param [out] network Network information + * @param [in] stats Pointer to routing netlink stats + * @param [in] duration Time from previous measurement + */ + void update_traffic( + NetworkInfomation & network, const struct rtnl_link_stats * stats, + const rclcpp::Duration & duration); + + /** + * @brief Update network information about CRC error + * @param [out] network Network information + * @param [in] stats Pointer to routing netlink stats + */ + void update_crc_error(NetworkInfomation & network, const struct rtnl_link_stats * stats); + + /** + * @brief Shutdown nl80211 object + */ + void shutdown_nl80211(); + + /** + * @brief Send request to start nethogs + */ + void send_start_nethogs_request(); + + /** + * @brief Get result of nethogs + * @param [out] result result of nethogs + */ + void get_nethogs_result(traffic_reader_service::Result & result); + + /** + * @brief Connect to traffic-reader service + * @return true on success, false on error + */ + bool connect_service(); + + /** + * @brief Send data to traffic-reader service + * @param [in] request Request to traffic-reader service + * @return true on success, false on error + */ + bool send_data(traffic_reader_service::Request request); /** - * @brief determine if it is a supported network - * @param [in] net_info network infomation - * @param [in] index index of network infomation index - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @param [out] error_str error string - * @return result of determining whether it is a supported network + * @brief Send data to traffic-reader service with parameters + * @param [in] request Request to traffic-reader service + * @param [in] parameters List of parameters + * @param[in] program_name Filter by program name + * @return true on success, false on error */ - bool isSupportedNetwork( - const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, - std::string & error_str); + bool send_data_with_parameters( + traffic_reader_service::Request request, std::vector & parameters, + std::string & program_name); /** - * @brief search column index of IP packet reassembles failed in /proc/net/snmp + * @brief Receive data from traffic-reader service + * @param [out] result Status from traffic-reader service */ - void searchReassemblesFailedColumnIndex(); + void receive_data(traffic_reader_service::Result & result); + + /** + * @brief Close connection with traffic-reader service + */ + void close_connection(); + + /** + * @brief Get column index of IP packet reassembles failed from `/proc/net/snmp` + */ + void get_reassembles_failed_column_index(); /** * @brief get IP packet reassembles failed * @param [out] reassembles_failed IP packet reassembles failed * @return execution result */ - bool getReassemblesFailed(uint64_t & reassembles_failed); + bool get_reassembles_failed(uint64_t & reassembles_failed); diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information - char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name - std::map bytes_; //!< @brief list of bytes - rclcpp::Time last_update_time_; //!< @brief last update time - std::vector device_params_; //!< @brief list of devices - NL80211 nl80211_; //!< @brief 802.11 netlink-based interface - int getifaddrs_errno_; //!< @brief errno set by getifaddrs() - std::vector net_info_list_; //!< @brief list of Network information + char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name + std::map bytes_; //!< @brief list of bytes + rclcpp::Time last_update_time_; //!< @brief last update time + std::vector device_params_; //!< @brief list of devices + NL80211 nl80211_; //!< @brief 802.11 netlink-based interface + int getifaddrs_error_code_; //!< @brief Error code set by getifaddrs() + std::vector network_list_; //!< @brief List of Network information - /** - * @brief CRC errors information - */ - typedef struct crc_errors - { - std::deque errors_queue; //!< @brief queue that holds count of CRC errors - unsigned int last_rx_crc_errors; //!< @brief rx_crc_error at the time of the last monitoring + std::string monitor_program_; //!< @brief nethogs monitor program name + std::string socket_path_; //!< @brief Path of UNIX domain socket + boost::asio::io_service io_service_; //!< @brief Core I/O functionality + std::unique_ptr acceptor_; //!< @brief UNIX domain acceptor + std::unique_ptr socket_; //!< @brief UNIX domain socket - crc_errors() : last_rx_crc_errors(0) {} - } crc_errors; - std::map crc_errors_; //!< @brief list of CRC errors + std::map crc_errors_; //!< @brief list of CRC errors + unsigned int crc_error_check_duration_; //!< @brief CRC error check duration + unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold std::deque reassembles_failed_queue_; //!< @brief queue that holds count of IP packet reassembles failed uint64_t last_reassembles_failed_; //!< @brief IP packet reassembles failed at the time of the //!< last monitoring - - std::string monitor_program_; //!< @brief nethogs monitor program name - bool nethogs_all_; //!< @brief nethogs result all mode - int traffic_reader_port_; //!< @brief port number to connect to traffic_reader - unsigned int crc_error_check_duration_; //!< @brief CRC error check duration - unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold unsigned int reassembles_failed_check_duration_; //!< @brief IP packet reassembles failed check duration unsigned int @@ -229,10 +312,16 @@ class NetMonitor : public rclcpp::Node unsigned int reassembles_failed_column_index_; //!< @brief column index of IP Reassembles failed //!< in /proc/net/snmp + /** + * @brief Network connection status messages + */ + const std::map connection_messages_ = { + {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "no such device"}, {DiagStatus::ERROR, "unused"}}; + /** * @brief Network usage status messages */ - const std::map usage_dict_ = { + const std::map usage_messages_ = { {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "high load"}, {DiagStatus::ERROR, "down"}}; }; diff --git a/system/system_monitor/include/traffic_reader/traffic_reader.hpp b/system/system_monitor/include/traffic_reader/traffic_reader_common.hpp similarity index 63% rename from system/system_monitor/include/traffic_reader/traffic_reader.hpp rename to system/system_monitor/include/traffic_reader/traffic_reader_common.hpp index 7b0276dee4c07..9d84072a42fc3 100644 --- a/system/system_monitor/include/traffic_reader/traffic_reader.hpp +++ b/system/system_monitor/include/traffic_reader/traffic_reader_common.hpp @@ -17,21 +17,33 @@ * @brief traffic reader definitions */ -#ifndef TRAFFIC_READER__TRAFFIC_READER_HPP_ -#define TRAFFIC_READER__TRAFFIC_READER_HPP_ +#ifndef TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ +#define TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ #include #include +#include #include +namespace traffic_reader_service +{ + +static constexpr char socket_path[] = "/tmp/traffic_reader"; + +enum Request { + NONE = 0, + START_NETHOGS, + GET_RESULT, +}; + /** - * @brief traffic information + * @brief Result of nethogs */ -struct TrafficReaderResult +struct Result { - int error_code_; //!< @brief error code, 0 on success, otherwise error - std::string str_; //!< @brief nethogs result string + int error_code; //!< @brief Error code, 0 on success, otherwise error + std::string output; //!< @brief Result output of nethogs /** * @brief Load or save data members. @@ -43,13 +55,13 @@ struct TrafficReaderResult template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { - ar & error_code_; - ar & str_; + ar & error_code; + ar & output; } }; -constexpr std::string_view GET_ALL_STR{""}; //!< @brief nethogs result all request string +// constexpr std::string_view GET_ALL_STR{""}; //!< @brief nethogs result all request string -constexpr int TRAFFIC_READER_PORT = 7636; //!< @brief traffic reader port: 7634-7647 Unassigned +} // namespace traffic_reader_service -#endif // TRAFFIC_READER__TRAFFIC_READER_HPP_ +#endif // TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ diff --git a/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp b/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp new file mode 100644 index 0000000000000..a66d0b121ed46 --- /dev/null +++ b/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp @@ -0,0 +1,110 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ +#define TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ + +#include "traffic_reader/traffic_reader_common.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace traffic_reader_service +{ + +namespace local = boost::asio::local; + +class TrafficReaderService +{ +public: + /** + * @brief Constructor + * @param[in] socket_path Path of UNIX domain socket + */ + explicit TrafficReaderService(std::string socket_path); + + /** + * @brief Initialization + * @return true on success, false on error + */ + bool initialize(); + + /** + * @brief Shutdown + */ + void shutdown(); + + /** + * @brief Main loop + */ + void run(); + +protected: + /** + * @brief Handle message + * @param[in] buffer Pointer to data received + */ + void handle_message(const char * buffer); + + /** + * @brief Start nethogs + * @param[in] archive Archive object for loading + */ + void start_nethogs(boost::archive::text_iarchive & archive); + + /** + * @brief Get command line of process from nethogs output + * @param[in] line nethogs output + * @return Command line of process + */ + static std::string get_command_line(const std::string & line); + + /** + * @brief Get command line of process from PID + * @param[in] pid PID + * @return Command line of process + */ + static std::string get_command_line_with_pid(pid_t pid); + + /** + * @brief Return result of nethogs + */ + void get_result(); + + /** + * @brief Execute nethogs + */ + void execute_nethogs(); + + std::string socket_path_; //!< @brief Path of UNIX domain socket + boost::asio::io_service io_service_; //!< @brief Core I/O functionality + std::unique_ptr acceptor_; //!< @brief UNIX domain acceptor + std::unique_ptr socket_; //!< @brief UNIX domain socket + std::thread thread_; //!< @brief Thread to run nethogs + std::mutex mutex_; //!< @brief Mutex guard for the flag + bool stop_; //!< @brief Flag to stop thread + std::vector devices_; //!< @brief List of devices + std::string program_name_; //!< @brief Filter by program name + traffic_reader_service::Result result_; //!< @brief Result of nethogs +}; + +} // namespace traffic_reader_service + +#endif // TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ diff --git a/system/system_monitor/launch/system_monitor.launch.xml b/system/system_monitor/launch/system_monitor.launch.xml index 0d8a14981bd8a..c6d29dd6b772f 100644 --- a/system/system_monitor/launch/system_monitor.launch.xml +++ b/system/system_monitor/launch/system_monitor.launch.xml @@ -1,12 +1,12 @@ - - - - - - - - + + + + + + + + diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader.cpp deleted file mode 100644 index ebb6d976d9921..0000000000000 --- a/system/system_monitor/reader/traffic_reader/traffic_reader.cpp +++ /dev/null @@ -1,351 +0,0 @@ -// Copyright 2021 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/** - * @file traffic_reader.cpp - * @brief traffic information read class - */ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace bp = boost::process; - -/** - * @brief thread safe nethogs result - */ -class NethogsResult -{ -public: - /** - * @brief get nethogs result string - * @param [out] str nethogs result string - * @return error code - */ - int get(std::string & str) - { - std::lock_guard lock(mutex_); - str = str_; - return err_; - } - /** - * @brief set nethogs result string and error code - * @param [in] str nethogs result string - * @param [in] err error code - * @return error code - */ - void set(const std::string & str, int err) - { - std::lock_guard lock(mutex_); - str_ = str; - err_ = err; - } - -private: - std::mutex mutex_; //!< @brief lock for thread safe - std::string str_; //!< @brief nethogs result string - int err_; //!< @brief error code, 0 on success, otherwise error -}; -static NethogsResult g_NethogsString; //!< @brief nethogs result -static volatile bool g_Running = false; //!< @brief run status, true running, false exit -static volatile int g_WatchDogCount = 0; //!< @brief executeNethogs watchdog, 0 refresh - -/** - * @brief print usage - */ -void usage() -{ - printf("Usage: traffic_reader [options]\n"); - printf(" -h --help : Display help\n"); - printf(" -p --port # : Port number to listen to.\n"); - printf("\n"); -} - -/** - * @brief thread function to execute nethogs - */ -void executeNethogs() -{ - const std::string cmd = "nethogs -t"; - // Output format of `nethogs -t` - // Blank Line | - // Start Line | Refreshing: - // Result Line 1 | PROGRAM/PID/UID SENT RECEIVED - // ... | ... - // Result Line n | PROGRAM/PID/UID SENT RECEIVED - // Last Line | unknown TCP/0/0 - std::string error_str; - std::string log_str; - - g_WatchDogCount = 0; - - while (g_Running) { - bp::ipstream is_out; - bp::ipstream is_err; - boost::process::child c; - try { - c = bp::child(cmd, bp::std_out > is_out, bp::std_err > is_err); - std::string result_buf; - std::string line; - while (std::getline(is_out, line)) { - if (!g_Running) { - // run() exit - return; - } - if (line.empty()) { - // Blank Line - if (!result_buf.empty()) { - g_NethogsString.set(result_buf, 0); - result_buf = ""; - } - } else if (line.find("unknown TCP/0/0") != std::string::npos) { - // Last Line - result_buf.append(line); - g_NethogsString.set(result_buf, 0); - result_buf = ""; - } else if (line.find("/0/0\t") != std::string::npos) { - // no-pid and root data skip. - } else if (line == std::string("Refreshing:")) { - // Start Line - g_WatchDogCount = 0; - if (!result_buf.empty()) { - g_NethogsString.set(result_buf, 0); - result_buf = ""; - } - if (!log_str.empty()) { - syslog(LOG_INFO, "[%s] command started.\n", cmd.c_str()); - log_str = ""; - } - } else { - // Result Line - result_buf.append(line + "\n"); - } - } // while - - c.wait(); - std::ostringstream os; - is_err >> os.rdbuf(); - error_str = "command terminate. " + os.str(); - } catch (boost::process::process_error & e) { - error_str = e.what(); - c.terminate(); - signal(SIGCHLD, SIG_IGN); - } - - int err_code = c.exit_code(); - // The err_code when killing a child process is usually 9. However, - // Because there were times when it was 0, 0 is changed to an error value. - if (err_code == 0) { - err_code = -1; - } - g_NethogsString.set("[" + cmd + "] " + error_str, err_code); - - if (log_str != error_str) { - log_str = error_str; - syslog(LOG_ERR, "[%s] err(%d) %s\n", cmd.c_str(), err_code, error_str.c_str()); - } - - for (int cnt = 10; cnt > 0 && g_Running; --cnt) { - g_WatchDogCount = 0; - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - } // while - - std::terminate(); -} - -/** - * @brief check NET temperature - * @param [in] port port to listen - */ -void run(int port) -{ - // Create a new socket - int sock = socket(AF_INET, SOCK_STREAM, 0); - if (sock < 0) { - syslog(LOG_ERR, "Failed to create a new socket. %s\n", strerror(errno)); - return; - } - - // Allow address reuse - int ret = 0; - int opt = 1; - ret = setsockopt( - sock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&opt), (socklen_t)sizeof(opt)); - if (ret < 0) { - syslog(LOG_ERR, "Failed to set socket FD's option. %s\n", strerror(errno)); - close(sock); - return; - } - - // Give the socket FD the local address ADDR - sockaddr_in addr{}; - addr.sin_family = AF_INET; - addr.sin_port = htons(port); - addr.sin_addr.s_addr = htonl(INADDR_ANY); - ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); - if (ret < 0) { - syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); - close(sock); - return; - } - - // Prepare to accept connections on socket FD - ret = listen(sock, 5); - if (ret < 0) { - syslog(LOG_ERR, "Failed to prepare to accept connections on socket FD. %s\n", strerror(errno)); - close(sock); - return; - } - - sockaddr_in client{}; - socklen_t len = sizeof(client); - - while (true) { - // Await a connection on socket FD - int new_sock = accept(sock, reinterpret_cast(&client), &len); - if (new_sock < 0) { - syslog( - LOG_ERR, "Failed to prepare to accept connections on socket FD. %s\n", strerror(errno)); - close(sock); - return; - } - - // Receive list of device from a socket - char buf[1024]{}; - ret = recv(new_sock, buf, sizeof(buf) - 1, 0); - if (ret < 0) { - syslog(LOG_ERR, "Failed to receive. %s\n", strerror(errno)); - close(new_sock); - close(sock); - return; - } - // No data received - if (ret == 0) { - syslog(LOG_ERR, "No data received. %s\n", strerror(errno)); - close(new_sock); - close(sock); - return; - } - - buf[sizeof(buf) - 1] = '\0'; - const std::string program_name = std::string(buf); - - // set result data - TrafficReaderResult result; - result.error_code_ = g_NethogsString.get(result.str_); - if (result.error_code_ == 0 && program_name != GET_ALL_STR) { - std::stringstream lines{result.str_}; - std::string line; - std::string result_str; - while (std::getline(lines, line)) { - if (line.find(program_name) != std::string::npos) { - result_str.append(line + "\n"); - } - } // while - result.str_ = result_str; - } - - std::ostringstream oss; - boost::archive::text_oarchive oa(oss); - oa << result; - // Write N bytes of BUF to FD - ret = write(new_sock, oss.str().c_str(), oss.str().length()); - if (ret < 0) { - syslog(LOG_ERR, "Failed to write N bytes of BUF to FD. %s\n", strerror(errno)); - } - - // Close the file descriptor FD - ret = close(new_sock); - if (ret < 0) { - syslog(LOG_ERR, "Failed to close the file descriptor FD. %s\n", strerror(errno)); - } - - if (++g_WatchDogCount >= 3) { - syslog(LOG_ERR, "nethogs command thread error\n"); - close(sock); - return; - } - } - - close(sock); -} - -int main(int argc, char ** argv) -{ - static struct option long_options[] = { - {"help", no_argument, nullptr, 'h'}, - {"port", required_argument, nullptr, 'p'}, - {nullptr, 0, nullptr, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = TRAFFIC_READER_PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; - - default: - break; - } - } - - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } - - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); - - g_Running = true; - std::thread th = std::thread(&executeNethogs); - th.detach(); - - run(port); - - g_Running = false; - - // Close descriptor used to write to system logger - closelog(); - - return EXIT_SUCCESS; -} diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp new file mode 100644 index 0000000000000..0af3b0d89d312 --- /dev/null +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp @@ -0,0 +1,89 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +/** + * @brief print usage + */ +void usage() +{ + printf("Usage: traffic_reader [options]\n"); + printf(" -h --help : Display help\n"); + printf(" -s --socket # : Path of UNIX domain socket.\n"); + printf("\n"); +} + +int main(int argc, char ** argv) +{ + static struct option long_options[] = { + {"help", no_argument, nullptr, 'h'}, + {"socket", required_argument, nullptr, 's'}, + {nullptr, 0, nullptr, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + std::string socket_path = traffic_reader_service::socket_path; + + while ((c = getopt_long(argc, argv, "hs:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 's': + socket_path = optarg; + break; + + default: + break; + } + } + + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } + + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); // NOLINT [hicpp-signed-bitwise] + + // Initialize traffic-reader service + traffic_reader_service::TrafficReaderService service(socket_path); + + if (!service.initialize()) { + service.shutdown(); + closelog(); + return EXIT_FAILURE; + } + + // Run main loop + service.run(); + + // Shutdown traffic-reader service + service.shutdown(); + + // Close descriptor used to write to system logger + closelog(); + + return EXIT_SUCCESS; +} diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp new file mode 100644 index 0000000000000..90d8867235e37 --- /dev/null +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp @@ -0,0 +1,288 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * @file traffic_reader_service.cpp + * @brief traffic information read class + */ + +#include + +#include +#include + +#include +#include + +namespace process = boost::process; + +namespace traffic_reader_service +{ + +TrafficReaderService::TrafficReaderService(std::string socket_path) +: socket_path_(std::move(socket_path)), stop_(false) +{ +} + +bool TrafficReaderService::initialize() +{ + // Remove previous binding + remove(socket_path_.c_str()); + + local::stream_protocol::endpoint endpoint(socket_path_); + acceptor_ = std::make_unique(io_service_, endpoint); + + // Run I/O service processing loop + boost::system::error_code error_code; + io_service_.run(error_code); + if (error_code) { + syslog(LOG_ERR, "Failed to run I/O service. %s\n", error_code.message().c_str()); + return false; + } + + // Give permission to other users to access to socket + // NOLINTNEXTLINE [hicpp-signed-bitwise] + if (chmod(socket_path_.c_str(), S_IRWXU | S_IRWXG | S_IRWXO) != 0) { + syslog(LOG_ERR, "Failed to give permission to unix domain socket. %s\n", strerror(errno)); + return false; + } + + return true; +} + +void TrafficReaderService::shutdown() { io_service_.stop(); } + +void TrafficReaderService::run() +{ + while (true) { + socket_ = std::make_unique(io_service_); + + // Accept a new connection + boost::system::error_code error_code; + acceptor_->accept(*socket_, error_code); + + if (error_code) { + syslog(LOG_ERR, "Failed to accept new connection. %s\n", error_code.message().c_str()); + socket_->close(); + continue; + } + + // Read data from socket + char buffer[1024]{}; + socket_->read_some(boost::asio::buffer(buffer, sizeof(buffer)), error_code); + + if (error_code) { + syslog(LOG_ERR, "Failed to read data from socket. %s\n", error_code.message().c_str()); + socket_->close(); + continue; + } + + // Handle message + handle_message(buffer); + + socket_->close(); + } +} + +void TrafficReaderService::handle_message(const char * buffer) +{ + uint8_t request_id = Request::NONE; + + // Restore request from ros node + std::istringstream in_stream(buffer); + boost::archive::text_iarchive archive(in_stream); + + try { + archive >> request_id; + } catch (const std::exception & e) { + syslog(LOG_ERR, "Failed to restore message. %s\n", e.what()); + return; + } + + switch (request_id) { + case Request::START_NETHOGS: + start_nethogs(archive); + break; + case Request::GET_RESULT: + get_result(); + break; + default: + syslog(LOG_WARNING, "Unknown message. %d\n", request_id); + break; + } +} + +void TrafficReaderService::start_nethogs(boost::archive::text_iarchive & archive) +{ + syslog(LOG_INFO, "Starting nethogs...\n"); + + // Restore list of devices from ros node + try { + archive >> devices_; + archive >> program_name_; + } catch (const std::exception & e) { + syslog(LOG_ERR, "Failed to restore optional commands. %s\n", e.what()); + devices_.clear(); + } + + // Stop thread if already running + if (thread_.joinable()) { + { + std::lock_guard lock(mutex_); + stop_ = true; + } + thread_.join(); + } + + // Run nethogs + stop_ = false; + thread_ = std::thread(&TrafficReaderService::execute_nethogs, this); +} + +void TrafficReaderService::get_result() +{ + // Inform ros node + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & Request::GET_RESULT; + { + std::lock_guard lock(mutex_); + archive & result_; + } + + // Write data to socket + boost::system::error_code error_code; + socket_->write_some( + boost::asio::buffer(out_stream.str().c_str(), out_stream.str().length()), error_code); + + if (error_code) { + syslog(LOG_ERR, "Failed to write data to socket. %s\n", error_code.message().c_str()); + } +} + +/** + * @brief thread function to execute nethogs + */ +void TrafficReaderService::execute_nethogs() +{ + std::stringstream command; + command << "nethogs -t"; + + for (const auto & device : devices_) { + command << " " << device; + } + + syslog(LOG_INFO, "%s\n", command.str().c_str()); + + process::ipstream is_out; + process::ipstream is_error; + std::string line; + + boost::process::child c(command.str(), process::std_out > is_out, process::std_err > is_error); + + // Output format of `nethogs -t [device(s)]` + // + // Refreshing: + // [PROGRAM/PID/UID SENT RECEIVED] + // usr/share/code/code/3102772/1000 0.565234 1.06855 + // ... + // unknown TCP/0/0 0 0 + while (std::getline(is_out, line)) { + // Exit loop when stop is requested + std::lock_guard lock(mutex_); + if (stop_) break; + + // Skip if line is empty + if (line.empty()) { + continue; + } + // Start of output + if (line == "Refreshing:") { + result_.error_code = EXIT_SUCCESS; + result_.output.clear(); + continue; + } + // Skip if no PID + if (line.find("/0/0\t") != std::string::npos) { + continue; + } + // End of output + if (line == "unknown TCP/0/0\t0\t0") { + continue; + } + + std::string command_line = get_command_line(line); + if (!command_line.empty()) { + // Add nethogs output and command line + if (program_name_ == "*" || command_line.find(program_name_) != std::string::npos) { + result_.output.append(fmt::format("{}\t{}\n", line, command_line)); + } + } + } + + c.terminate(); + + std::ostringstream out_stream; + is_error >> out_stream.rdbuf(); + + // Remove new line from output + std::string message = out_stream.str(); + message.erase(std::remove(message.begin(), message.end(), '\n'), message.cend()); + syslog(LOG_INFO, "%s\n", message.c_str()); + + { + std::lock_guard lock(mutex_); + result_.error_code = c.exit_code(); + result_.output = message; + } +} + +std::string TrafficReaderService::get_command_line(const std::string & line) +{ + std::vector tag_tokens; + std::vector slash_tokens; + + // usr/share/code/code/3102772/1000 0.565234 1.06855 + // Get first token separated by tab + boost::split(tag_tokens, line, boost::is_any_of("\t"), boost::token_compress_on); + + // usr/share/code/code/3102772/1000 + // Get second from the last separated by slash + boost::split( + slash_tokens, tag_tokens[0].c_str(), boost::is_any_of("/"), boost::token_compress_on); + if (slash_tokens.size() >= 3) { + return get_command_line_with_pid(std::atoi(slash_tokens[slash_tokens.size() - 2].c_str())); + } + + return ""; +} + +std::string TrafficReaderService::get_command_line_with_pid(pid_t pid) +{ + std::ifstream file(fmt::format("/proc/{}/cmdline", pid)); + + if (!file) { + return ""; + } + + std::string line; + getline(file, line); + + // cmdline is null separated, replace with space + std::replace(line.begin(), line.end(), '\0', ' '); + + return line; +} + +} // namespace traffic_reader_service diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index d9a42f8c03835..4ceec63ca862c 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -21,499 +21,268 @@ #include "system_monitor/system_monitor_utility.hpp" -#include +#include +#include #include -#include -// #include // workaround for build errors +#include +#define FMT_HEADER_ONLY #include #include #include #include #include -#include -#include -#include - -#include -#include -#include -#include -#include NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) : Node("net_monitor", options), updater_(this), + hostname_(), last_update_time_{0, 0, this->get_clock()->get_clock_type()}, device_params_( declare_parameter>("devices", std::vector())), - last_reassembles_failed_(0), + getifaddrs_error_code_(0), monitor_program_(declare_parameter("monitor_program", "greengrass")), - traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)), + socket_path_(declare_parameter("socket_path", traffic_reader_service::socket_path)), crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)), + last_reassembles_failed_(0), reassembles_failed_check_duration_( declare_parameter("reassembles_failed_check_duration", 1)), reassembles_failed_check_count_(declare_parameter("reassembles_failed_check_count", 1)), reassembles_failed_column_index_(0) { - using namespace std::literals::chrono_literals; - if (monitor_program_.empty()) { - monitor_program_ = GET_ALL_STR; - nethogs_all_ = true; - } else { - nethogs_all_ = false; + monitor_program_ = "*"; } gethostname(hostname_, sizeof(hostname_)); updater_.setHardwareID(hostname_); - updater_.add("Network Usage", this, &NetMonitor::checkUsage); - updater_.add("Network Traffic", this, &NetMonitor::monitorTraffic); - updater_.add("Network CRC Error", this, &NetMonitor::checkCrcError); - updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::checkReassemblesFailed); + updater_.add("Network Connection", this, &NetMonitor::check_connection); + updater_.add("Network Usage", this, &NetMonitor::check_usage); + updater_.add("Network Traffic", this, &NetMonitor::monitor_traffic); + updater_.add("Network CRC Error", this, &NetMonitor::check_crc_error); + updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::check_reassembles_failed); nl80211_.init(); - searchReassemblesFailedColumnIndex(); + // Run I/O service processing loop + boost::system::error_code error_code; + io_service_.run(error_code); + if (error_code) { + RCLCPP_WARN(get_logger(), "Failed to run I/O service. %s", error_code.message().c_str()); + } - // get Network information for the first time - updateNetworkInfoList(); + // Update list of network information + update_network_list(); + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::on_timer, this)); - timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::onTimer, this)); + // Get column index of IP packet reassembles failed from `/proc/net/snmp` + get_reassembles_failed_column_index(); + + // Send request to start nethogs + send_start_nethogs_request(); } NetMonitor::~NetMonitor() { shutdown_nl80211(); } -void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } - -void NetMonitor::onTimer() { updateNetworkInfoList(); } - -// cspell: ignore ifas, ifrm, ifrc -void NetMonitor::updateNetworkInfoList() +void NetMonitor::check_connection(diagnostic_updater::DiagnosticStatusWrapper & status) { - net_info_list_.clear(); + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); if (device_params_.empty()) { + status.summary(DiagStatus::ERROR, "invalid device parameter"); return; } - - const struct ifaddrs * ifa; - struct ifaddrs * ifas = nullptr; - - rclcpp::Duration duration = this->now() - last_update_time_; - - // Get network interfaces - getifaddrs_errno_ = 0; - if (getifaddrs(&ifas) < 0) { - getifaddrs_errno_ = errno; + if (getifaddrs_error_code_ != 0) { + status.summary(DiagStatus::ERROR, "getifaddrs error"); + status.add("getifaddrs", strerror(getifaddrs_error_code_)); return; } - for (ifa = ifas; ifa; ifa = ifa->ifa_next) { - // Skip no addr - if (!ifa->ifa_addr) { - continue; - } - // Skip loopback - if (ifa->ifa_flags & IFF_LOOPBACK) { - continue; - } - // Skip non AF_PACKET - if (ifa->ifa_addr->sa_family != AF_PACKET) { - continue; - } - // Skip device not specified - if ( - boost::find(device_params_, ifa->ifa_name) == device_params_.end() && - boost::find(device_params_, "*") == device_params_.end()) { - continue; - } - - int fd; - struct ifreq ifrm; - struct ifreq ifrc; - struct ethtool_cmd edata; - - net_info_list_.emplace_back(); - auto & net_info = net_info_list_.back(); - - net_info.interface_name = std::string(ifa->ifa_name); - - // Get MTU information - fd = socket(AF_INET, SOCK_DGRAM, 0); - strncpy(ifrm.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); - if (ioctl(fd, SIOCGIFMTU, &ifrm) < 0) { - net_info.mtu_errno = errno; - close(fd); - continue; - } + int index = 0; + int whole_level = DiagStatus::OK; + std::string error_message; - // Get network capacity - strncpy(ifrc.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); - ifrc.ifr_data = (caddr_t)&edata; - edata.cmd = ETHTOOL_GSET; - if (ioctl(fd, SIOCETHTOOL, &ifrc) < 0) { - // possibly wireless connection, get bitrate(MBit/s) - net_info.speed = nl80211_.getBitrate(ifa->ifa_name); - if (net_info.speed <= 0) { - net_info.ethtool_errno = errno; - close(fd); - continue; - } + for (const auto & network : network_list_) { + if (network.is_invalid) { + make_invalid_diagnostic_status(network, index, status, error_message); } else { - net_info.speed = edata.speed; - } - - net_info.is_running = (ifa->ifa_flags & IFF_RUNNING); - - auto * stats = (struct rtnl_link_stats *)ifa->ifa_data; - if (bytes_.find(net_info.interface_name) != bytes_.end()) { - net_info.rx_traffic = - toMbit(stats->rx_bytes - bytes_[net_info.interface_name].rx_bytes) / duration.seconds(); - net_info.tx_traffic = - toMbit(stats->tx_bytes - bytes_[net_info.interface_name].tx_bytes) / duration.seconds(); - net_info.rx_usage = net_info.rx_traffic / net_info.speed; - net_info.tx_usage = net_info.tx_traffic / net_info.speed; + status.add(fmt::format("Network {}: status", index), connection_messages_.at(DiagStatus::OK)); + status.add(fmt::format("Network {}: name", index), network.interface_name); } + ++index; + } - net_info.mtu = ifrm.ifr_mtu; - net_info.rx_bytes = stats->rx_bytes; - net_info.rx_errors = stats->rx_errors; - net_info.tx_bytes = stats->tx_bytes; - net_info.tx_errors = stats->tx_errors; - net_info.collisions = stats->collisions; - - close(fd); + // Check if specified device exists + for (const auto & device : device_params_) { + // Skip if device not specified + if (device == "*") continue; - bytes_[net_info.interface_name].rx_bytes = stats->rx_bytes; - bytes_[net_info.interface_name].tx_bytes = stats->tx_bytes; + // Check if device exists in detected networks + const auto object = std::find_if( + network_list_.begin(), network_list_.end(), + [&device](const auto & network) { return network.interface_name == device; }); - // Get the count of CRC errors - crc_errors & crc_ers = crc_errors_[net_info.interface_name]; - crc_ers.errors_queue.push_back(stats->rx_crc_errors - crc_ers.last_rx_crc_errors); - while (crc_ers.errors_queue.size() > crc_error_check_duration_) { - crc_ers.errors_queue.pop_front(); + if (object == network_list_.end()) { + whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); + error_message = "no such device"; + status.add( + fmt::format("Network {}: status", index), connection_messages_.at(DiagStatus::WARN)); + status.add(fmt::format("Network {}: name", index), device); } - crc_ers.last_rx_crc_errors = stats->rx_crc_errors; + + ++index; } - freeifaddrs(ifas); + if (!error_message.empty()) { + status.summary(whole_level, error_message); + } else { + status.summary(whole_level, connection_messages_.at(whole_level)); + } - last_update_time_ = this->now(); + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::check_usage(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!checkGeneralInfo(stat)) { - return; - } - int level = DiagStatus::OK; - int whole_level = DiagStatus::OK; int index = 0; std::string error_str; std::vector interface_names; - for (const auto & net_info : net_info_list_) { - if (!isSupportedNetwork(net_info, index, stat, error_str)) { - ++index; - interface_names.push_back(net_info.interface_name); - continue; - } - - level = net_info.is_running ? DiagStatus::OK : DiagStatus::ERROR; - - stat.add(fmt::format("Network {}: status", index), usage_dict_.at(level)); - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", net_info.rx_usage * 1e+2); - stat.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", net_info.tx_usage * 1e+2); - stat.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", net_info.rx_traffic); - stat.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", net_info.tx_traffic); - stat.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", net_info.speed); - stat.add(fmt::format("Network {}: mtu", index), net_info.mtu); - stat.add(fmt::format("Network {}: rx_bytes", index), net_info.rx_bytes); - stat.add(fmt::format("Network {}: rx_errors", index), net_info.rx_errors); - stat.add(fmt::format("Network {}: tx_bytes", index), net_info.tx_bytes); - stat.add(fmt::format("Network {}: tx_errors", index), net_info.tx_errors); - stat.add(fmt::format("Network {}: collisions", index), net_info.collisions); + for (const auto & network : network_list_) { + // Skip if network is not supported + if (network.is_invalid) continue; + + level = network.is_running ? DiagStatus::OK : DiagStatus::ERROR; + + status.add(fmt::format("Network {}: status", index), usage_messages_.at(level)); + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", network.rx_usage * 1e+2); + status.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", network.tx_usage * 1e+2); + status.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", network.rx_traffic); + status.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", network.tx_traffic); + status.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", network.speed); + status.add(fmt::format("Network {}: mtu", index), network.mtu); + status.add(fmt::format("Network {}: rx_bytes", index), network.rx_bytes); + status.add(fmt::format("Network {}: rx_errors", index), network.rx_errors); + status.add(fmt::format("Network {}: tx_bytes", index), network.tx_bytes); + status.add(fmt::format("Network {}: tx_errors", index), network.tx_errors); + status.add(fmt::format("Network {}: collisions", index), network.collisions); ++index; - - interface_names.push_back(net_info.interface_name); } - // Check if specified device exists - for (const auto & device : device_params_) { - // Skip if all devices specified - if (device == "*") { - continue; - } - // Skip if device already appended - if (boost::find(interface_names, device) != interface_names.end()) { - continue; - } - - stat.add(fmt::format("Network {}: status", index), "No Such Device"); - stat.add(fmt::format("Network {}: interface name", index), device); - error_str = "no such device"; - ++index; - } - - if (!error_str.empty()) { - stat.summary(DiagStatus::ERROR, error_str); - } else { - stat.summary(whole_level, usage_dict_.at(whole_level)); - } + status.summary(DiagStatus::OK, "OK"); // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::checkCrcError(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::check_crc_error(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!checkGeneralInfo(stat)) { - return; - } - int whole_level = DiagStatus::OK; int index = 0; - std::string error_str; + std::string error_message; - for (const auto & net_info : net_info_list_) { - if (!isSupportedNetwork(net_info, index, stat, error_str)) { - ++index; - continue; - } + for (const auto & network : network_list_) { + // Skip if network is not supported + if (network.is_invalid) continue; - crc_errors & crc_ers = crc_errors_[net_info.interface_name]; + CrcErrors & crc_errors = crc_errors_[network.interface_name]; unsigned int unit_rx_crc_errors = 0; - for (auto errors : crc_ers.errors_queue) { + for (auto errors : crc_errors.errors_queue) { unit_rx_crc_errors += errors; } - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.add(fmt::format("Network {}: total rx_crc_errors", index), crc_ers.last_rx_crc_errors); - stat.add(fmt::format("Network {}: rx_crc_errors per unit time", index), unit_rx_crc_errors); + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.add( + fmt::format("Network {}: total rx_crc_errors", index), crc_errors.last_rx_crc_errors); + status.add(fmt::format("Network {}: rx_crc_errors per unit time", index), unit_rx_crc_errors); if (unit_rx_crc_errors >= crc_error_count_threshold_) { whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); - error_str = "CRC error"; + error_message = "CRC error"; } ++index; } - if (!error_str.empty()) { - stat.summary(whole_level, error_str); + if (!error_message.empty()) { + status.summary(whole_level, error_message); } else { - stat.summary(whole_level, "OK"); + status.summary(whole_level, "OK"); } // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -bool NetMonitor::checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - if (device_params_.empty()) { - stat.summary(DiagStatus::ERROR, "invalid device parameter"); - return false; - } - - if (getifaddrs_errno_ != 0) { - stat.summary(DiagStatus::ERROR, "getifaddrs error"); - stat.add("getifaddrs", strerror(getifaddrs_errno_)); - return false; - } - return true; -} - -bool NetMonitor::isSupportedNetwork( - const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, - std::string & error_str) -{ - // MTU information - if (net_info.mtu_errno != 0) { - if (net_info.mtu_errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.add("ioctl(SIOCGIFMTU)", strerror(net_info.mtu_errno)); - return false; - } - - // network capacity - if (net_info.speed <= 0) { - if (net_info.ethtool_errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.add("ioctl(SIOCETHTOOL)", strerror(net_info.ethtool_errno)); - return false; - } - return true; -} - -#include // workaround for build errors - -void NetMonitor::monitorTraffic(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::monitor_traffic(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - // Create a new socket - int sock = socket(AF_INET, SOCK_STREAM, 0); - if (sock < 0) { - stat.summary(DiagStatus::ERROR, "socket error"); - stat.add("socket", strerror(errno)); - return; - } - - // Specify the receiving timeouts until reporting an error - struct timeval tv; - tv.tv_sec = 10; - tv.tv_usec = 0; - int ret = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "setsockopt error"); - stat.add("setsockopt", strerror(errno)); - close(sock); - return; - } - - // Connect the socket referred to by the file descriptor - sockaddr_in addr; - memset(&addr, 0, sizeof(sockaddr_in)); - addr.sin_family = AF_INET; - addr.sin_port = htons(traffic_reader_port_); - addr.sin_addr.s_addr = htonl(INADDR_ANY); - ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "connect error"); - stat.add("connect", strerror(errno)); - close(sock); - return; - } - - // Write list of devices to FD - ret = write(sock, monitor_program_.c_str(), monitor_program_.length()); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "write error"); - stat.add("write", strerror(errno)); - RCLCPP_ERROR(get_logger(), "write error"); - close(sock); - return; - } - - // Receive messages from a socket - std::string rcv_str; - char buf[16 * 1024 + 1]; - do { - ret = recv(sock, buf, sizeof(buf) - 1, 0); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "recv error"); - stat.add("recv", strerror(errno)); - close(sock); - return; - } - if (ret > 0) { - buf[ret] = '\0'; - rcv_str += std::string(buf); - } - } while (ret > 0); - - // Close the file descriptor FD - ret = close(sock); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "close error"); - stat.add("close", strerror(errno)); - return; - } - - // No data received - if (rcv_str.length() == 0) { - stat.summary(DiagStatus::ERROR, "recv error"); - stat.add("recv", "No data received"); - return; - } - - // Restore information list - TrafficReaderResult result; - try { - std::istringstream iss(rcv_str); - boost::archive::text_iarchive oa(iss); - oa >> result; - } catch (const std::exception & e) { - stat.summary(DiagStatus::ERROR, "recv error"); - stat.add("recv", e.what()); - return; - } + // Get result of nethogs + traffic_reader_service::Result result; + get_nethogs_result(result); // traffic_reader result to output - if (result.error_code_ != 0) { - stat.summary(DiagStatus::ERROR, "traffic_reader error"); - stat.add("error", result.str_); + if (result.error_code != EXIT_SUCCESS) { + status.summary(DiagStatus::ERROR, "traffic_reader error"); + status.add("error", result.output); } else { - stat.summary(DiagStatus::OK, "OK"); + status.summary(DiagStatus::OK, "OK"); - if (result.str_.length() == 0) { - stat.add("nethogs: result", "nothing"); - } else if (nethogs_all_) { - stat.add("nethogs: all (KB/sec):", result.str_); + if (result.output.empty()) { + status.add("nethogs: result", fmt::format("No data monitored: {}", monitor_program_)); } else { - std::stringstream lines{result.str_}; + std::stringstream lines{result.output}; std::string line; std::vector list; - int idx = 0; + int index = 0; while (std::getline(lines, line)) { - if (line.empty()) { - continue; - } + if (line.empty()) continue; + boost::split(list, line, boost::is_any_of("\t"), boost::token_compress_on); if (list.size() >= 3) { - stat.add(fmt::format("nethogs {}: PROGRAM", idx), list[0].c_str()); - stat.add(fmt::format("nethogs {}: SENT (KB/sec)", idx), list[1].c_str()); - stat.add(fmt::format("nethogs {}: RECEIVED (KB/sec)", idx), list[2].c_str()); + status.add(fmt::format("nethogs {}: program", index), list[3].c_str()); + status.add(fmt::format("nethogs {}: sent (KB/s)", index), list[1].c_str()); + status.add(fmt::format("nethogs {}: received (KB/sec)", index), list[2].c_str()); } else { - stat.add(fmt::format("nethogs {}: result", idx), line); + status.add(fmt::format("nethogs {}: result", index), line); } - idx++; - } // while + ++index; + } } } // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::checkReassemblesFailed(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::check_reassembles_failed(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); int whole_level = DiagStatus::OK; - std::string error_str; + std::string error_message; uint64_t total_reassembles_failed = 0; uint64_t unit_reassembles_failed = 0; - if (getReassemblesFailed(total_reassembles_failed)) { + if (get_reassembles_failed(total_reassembles_failed)) { reassembles_failed_queue_.push_back(total_reassembles_failed - last_reassembles_failed_); while (reassembles_failed_queue_.size() > reassembles_failed_check_duration_) { reassembles_failed_queue_.pop_front(); @@ -523,32 +292,226 @@ void NetMonitor::checkReassemblesFailed(diagnostic_updater::DiagnosticStatusWrap unit_reassembles_failed += reassembles_failed; } - stat.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); - stat.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); + status.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); + status.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); if (unit_reassembles_failed >= reassembles_failed_check_count_) { whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); - error_str = "reassembles failed"; + error_message = "reassembles failed"; } last_reassembles_failed_ = total_reassembles_failed; } else { reassembles_failed_queue_.push_back(0); whole_level = std::max(whole_level, static_cast(DiagStatus::ERROR)); - error_str = "failed to read /proc/net/snmp"; + error_message = "failed to read /proc/net/snmp"; } - if (!error_str.empty()) { - stat.summary(whole_level, error_str); + if (!error_message.empty()) { + status.summary(whole_level, error_message); } else { - stat.summary(whole_level, "OK"); + status.summary(whole_level, "OK"); } // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::searchReassemblesFailedColumnIndex() +void NetMonitor::on_timer() +{ + // Update list of network information + update_network_list(); +} + +void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } + +void NetMonitor::make_invalid_diagnostic_status( + const NetworkInfomation & network, int index, + diagnostic_updater::DiagnosticStatusWrapper & status, std::string & error_message) +{ + // MTU information + if (network.mtu_error_code != 0) { + if (network.mtu_error_code == ENOTSUP) { + status.add(fmt::format("Network {}: status", index), "Not Supported"); + // Assume no error, no error message + } else { + status.add(fmt::format("Network {}: status", index), "Error"); + error_message = "ioctl error"; + } + + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.add("ioctl(SIOCGIFMTU)", strerror(network.mtu_error_code)); + return; + } + + // network capacity + if (network.speed <= 0) { + if (network.ethtool_error_code == ENOTSUP) { + status.add(fmt::format("Network {}: status", index), "Not Supported"); + // Assume no error, no error message + } else { + status.add(fmt::format("Network {}: status", index), "Error"); + error_message = "ioctl error"; + } + + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.add("ioctl(SIOCETHTOOL)", strerror(network.ethtool_error_code)); + } +} + +void NetMonitor::update_network_list() +{ + rclcpp::Duration duration = this->now() - last_update_time_; + + struct ifaddrs * interfaces = {}; + network_list_.clear(); + + // Get network interfaces + if (getifaddrs(&interfaces) < 0) { + getifaddrs_error_code_ = errno; + return; + } + + for (const auto * interface = interfaces; interface; interface = interface->ifa_next) { + // Skip no addr + if (!interface->ifa_addr) { + continue; + } + // Skip loopback + if (interface->ifa_flags & IFF_LOOPBACK) { + continue; + } + // Skip non AF_PACKET + if (interface->ifa_addr->sa_family != AF_PACKET) { + continue; + } + // Skip device not specified + const auto object = std::find_if( + device_params_.begin(), device_params_.end(), + [&interface](const auto & device) { return device == "*" || device == interface->ifa_name; }); + if (object == device_params_.end()) { + continue; + } + + NetworkInfomation network{}; + network.interface_name = interface->ifa_name; + network.is_running = (interface->ifa_flags & IFF_RUNNING); + + // Update network information using socket + update_network_information_by_socket(network); + + // Update network information using routing netlink stats + update_network_information_by_routing_netlink(network, interface->ifa_data, duration); + + network_list_.emplace_back(network); + } + + freeifaddrs(interfaces); + + last_update_time_ = this->now(); +} + +void NetMonitor::update_network_information_by_socket(NetworkInfomation & network) +{ + // Get MTU information + int fd = socket(AF_INET, SOCK_DGRAM, 0); + + // Update MTU information + update_mtu(network, fd); + + // Update network capacity + update_network_capacity(network, fd); + + close(fd); +} + +void NetMonitor::update_mtu(NetworkInfomation & network, int socket) +{ + struct ifreq request = {}; + + // NOLINTNEXTLINE [cppcoreguidelines-pro-type-union-access] + strncpy(request.ifr_name, network.interface_name.c_str(), IFNAMSIZ - 1); + if (ioctl(socket, SIOCGIFMTU, &request) < 0) { + network.is_invalid = true; + network.mtu_error_code = errno; + return; + } + + network.mtu = request.ifr_mtu; // NOLINT [cppcoreguidelines-pro-type-union-access] +} + +void NetMonitor::update_network_capacity(NetworkInfomation & network, int socket) +{ + struct ifreq request = {}; + struct ethtool_cmd ether_request = {}; + + // NOLINTNEXTLINE [cppcoreguidelines-pro-type-union-access] + strncpy(request.ifr_name, network.interface_name.c_str(), IFNAMSIZ - 1); + request.ifr_data = (caddr_t)ðer_request; // NOLINT [cppcoreguidelines-pro-type-cstyle-cast] + + ether_request.cmd = ETHTOOL_GSET; + if (ioctl(socket, SIOCETHTOOL, &request) >= 0) { + network.speed = ether_request.speed; + return; + } + + // Possibly wireless connection, get bitrate(MBit/s) + float ret = nl80211_.getBitrate(network.interface_name.c_str()); + if (ret <= 0) { + network.is_invalid = true; + network.ethtool_error_code = errno; + } else { + network.speed = ret; + } +} + +void NetMonitor::update_network_information_by_routing_netlink( + NetworkInfomation & network, void * data, const rclcpp::Duration & duration) +{ + auto * stats = static_cast(data); + + update_traffic(network, stats, duration); + + update_crc_error(network, stats); +} + +void NetMonitor::update_traffic( + NetworkInfomation & network, const struct rtnl_link_stats * stats, + const rclcpp::Duration & duration) +{ + network.rx_bytes = stats->rx_bytes; + network.rx_errors = stats->rx_errors; + network.tx_bytes = stats->tx_bytes; + network.tx_errors = stats->tx_errors; + network.collisions = stats->collisions; + + // Calculate traffic and usage if interface is entried in bytes + const auto bytes_entry = bytes_.find(network.interface_name); + if (bytes_entry != bytes_.end()) { + network.rx_traffic = + to_mbit(stats->rx_bytes - bytes_entry->second.rx_bytes) / duration.seconds(); + network.tx_traffic = + to_mbit(stats->tx_bytes - bytes_entry->second.tx_bytes) / duration.seconds(); + network.rx_usage = network.rx_traffic / network.speed; + network.tx_usage = network.tx_traffic / network.speed; + } + + bytes_[network.interface_name].rx_bytes = stats->rx_bytes; + bytes_[network.interface_name].tx_bytes = stats->tx_bytes; +} + +void NetMonitor::update_crc_error(NetworkInfomation & network, const struct rtnl_link_stats * stats) +{ + // Get the count of CRC errors + CrcErrors & crc_errors = crc_errors_[network.interface_name]; + crc_errors.errors_queue.push_back(stats->rx_crc_errors - crc_errors.last_rx_crc_errors); + while (crc_errors.errors_queue.size() > crc_error_check_duration_) { + crc_errors.errors_queue.pop_front(); + } + crc_errors.last_rx_crc_errors = stats->rx_crc_errors; +} + +void NetMonitor::get_reassembles_failed_column_index() { std::ifstream ifs("/proc/net/snmp"); if (!ifs) { @@ -556,45 +519,43 @@ void NetMonitor::searchReassemblesFailedColumnIndex() return; } - // /proc/net/snmp - // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... - // Ip: 2 64 5636471397 ... 135 2303339 216166 270 ... - std::string line; - // Find column index of 'ReasmFails' + std::string line; if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp first line."); + RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); return; } - std::vector title_list; - boost::split(title_list, line, boost::is_space()); + // /proc/net/snmp + // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... + // Ip: 2 64 5636471397 ... 135 2303339 216166 270 .. + std::vector header_list; + boost::split(header_list, line, boost::is_space()); - if (title_list.empty()) { - RCLCPP_WARN(get_logger(), "/proc/net/snmp first line is empty."); + if (header_list.empty()) { + RCLCPP_WARN(get_logger(), "Failed to get header list of /proc/net/snmp."); return; } - if (title_list[0] != "Ip:") { + if (header_list[0] != "Ip:") { RCLCPP_WARN( - get_logger(), "/proc/net/snmp line title column is invalid. : %s", title_list[0].c_str()); + get_logger(), "Header column is invalid in /proc/net/snmp. %s", header_list[0].c_str()); return; } int index = 0; - for (auto itr = title_list.begin(); itr != title_list.end(); ++itr, ++index) { - if (*itr == "ReasmFails") { + for (const auto & header : header_list) { + if (header == "ReasmFails") { reassembles_failed_column_index_ = index; break; } + ++index; } } -bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) +bool NetMonitor::get_reassembles_failed(uint64_t & reassembles_failed) { if (reassembles_failed_column_index_ == 0) { - RCLCPP_WARN( - get_logger(), "reassembles failed column index is invalid. : %d", - reassembles_failed_column_index_); + RCLCPP_WARN(get_logger(), "Column index is invalid. : %d", reassembles_failed_column_index_); return false; } @@ -606,15 +567,15 @@ bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) std::string line; - // Skip title row + // Skip header row if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp first line."); + RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); return false; } // Find a value of 'ReasmFails' if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp second line."); + RCLCPP_WARN(get_logger(), "Failed to get a line of /proc/net/snmp."); return false; } @@ -634,5 +595,143 @@ bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) return true; } +void NetMonitor::send_start_nethogs_request() +{ + // Connect to boot/shutdown service + if (!connect_service()) { + close_connection(); + return; + } + + diagnostic_updater::DiagnosticStatusWrapper stat; + std::vector interface_names; + + for (const auto & network : network_list_) { + // Skip if network is not supported + if (network.is_invalid) continue; + + interface_names.push_back(network.interface_name); + } + + // Send data to traffic-reader service + send_data_with_parameters( + traffic_reader_service::START_NETHOGS, interface_names, monitor_program_); + + // Close connection with traffic-reader service + close_connection(); +} + +void NetMonitor::get_nethogs_result(traffic_reader_service::Result & result) +{ + // Connect to traffic-reader service + if (!connect_service()) { + close_connection(); + return; + } + + // Send data to traffic-reader service + if (!send_data(traffic_reader_service::Request::GET_RESULT)) { + close_connection(); + return; + } + + // Receive data from traffic-reader service + receive_data(result); + + // Close connection with traffic-reader service + close_connection(); +} + +bool NetMonitor::connect_service() +{ + local::stream_protocol::endpoint endpoint(socket_path_); + socket_ = std::make_unique(io_service_); + + // Connect socket + boost::system::error_code error_code; + socket_->connect(endpoint, error_code); + + if (error_code) { + RCLCPP_ERROR(get_logger(), "Failed to connect socket. %s", error_code.message().c_str()); + return false; + } + + return true; +} + +bool NetMonitor::send_data(traffic_reader_service::Request request) +{ + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & request; + + // Write data to socket + boost::system::error_code error_code; + socket_->write_some( + boost::asio::buffer(out_stream.str().c_str(), out_stream.str().length()), error_code); + + if (error_code) { + RCLCPP_ERROR(get_logger(), "Failed to write data to socket. %s", error_code.message().c_str()); + return false; + } + + return true; +} + +bool NetMonitor::send_data_with_parameters( + traffic_reader_service::Request request, std::vector & parameters, + std::string & program_name) +{ + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & request; + archive & parameters; + archive & program_name; + + // Write data to socket + boost::system::error_code error_code; + socket_->write_some( + boost::asio::buffer(out_stream.str().c_str(), out_stream.str().length()), error_code); + + if (error_code) { + RCLCPP_ERROR(get_logger(), "Failed to write data to socket. %s", error_code.message().c_str()); + return false; + } + + return true; +} + +void NetMonitor::receive_data(traffic_reader_service::Result & result) +{ + uint8_t request_id = traffic_reader_service::Request::NONE; + + // Read data from socket + char buffer[10240]{}; + boost::system::error_code error_code; + socket_->read_some(boost::asio::buffer(buffer, sizeof(buffer)), error_code); + + if (error_code) { + RCLCPP_ERROR( + get_logger(), "Failed to read data from socket. %s\n", error_code.message().c_str()); + return; + } + + // Restore device status list + try { + std::istringstream in_stream(buffer); + boost::archive::text_iarchive archive(in_stream); + archive >> request_id; + archive >> result; + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to restore message. %s\n", e.what()); + } +} + +void NetMonitor::close_connection() +{ + // Close socket + socket_->close(); +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(NetMonitor) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md index 0532b6f474818..3c17acb300a18 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md @@ -181,3 +181,76 @@ ros2 run accel_brake_map_calibrator actuation_cmd_publisher.py ``` ![actuation_cmd_publisher_util](./media/actuation_cmd_publisher_util.png) + +## Calibration Method + +Two algorithms are selectable for the acceleration map update, [update_offset_four_cell_around](#update_offset_each_cell) and [update_offset_each_cell](update_offset_each_cell). Please see the link for datails. + +### Data Preprocessing + +Before calibration, missing or unusable data (e.g., too large handle angles) must first be eliminated. The following parameters are used to determine which data to remove. + +#### Parameters + +| Name | Description | Default Value | +| ---------------------- | ---------------------------- | ------------- | +| velocity_min_threshold | Exclude minimal velocity | 0.1 | +| max_steer_threshold | Exclude large steering angle | 0.2 | +| max_pitch_threshold | Exclude large pitch angle | 0.02 | +| max_jerk_threshold | Exclude large jerk | 0.7 | +| pedal_velocity_thresh | Exclude large pedaling speed | 0.15 | + +### update_offset_each_cell + +Update by Recursive Least Squares(RLS) method using data close enough to each grid. + +**Advantage** : Only data close enough to each grid is used for calibration, allowing accurate updates at each point. + +**Disadvantage** : Calibration is time-consuming due to a large amount of data to be excluded. + +#### Parameters + +Data selection is determined by the following thresholds. +| Name | Default Value | +| -------- | -------- | +|velocity_diff_threshold|0.556| +|pedal_diff_threshold|0.03| + +#### Update formula + +$$ +\begin{align} + \theta[n]=& + \theta[n-1]+\frac{p[n-1]x^{(n)}}{\lambda+p[n-1](x^{(n)})^2}(y^{(n)}-\theta[n-1]x^{(n)})\\ + p[n]=&\frac{p[n-1]}{\lambda+p[n-1](x^{(n)})^2} +\end{align} +$$ + +#### Variables + +| Variable name | Symbol | +| ------------------ | ----------- | +| covariance | $p[n-1]$ | +| map_offset | $\theta[n]$ | +| forgetting*factor* | $\lambda$ | +| phi | $x(=1)$ | +| measured_acc | $y$ | + +### update_offset_four_cell_around [1] + +Update the offsets by RLS in four grids around newly obtained data. By considering linear interpolation, the update takes into account appropriate weights. Therefore, there is no need to remove data by thresholding. + +**Advantage** : No data is wasted because updates are performed on the 4 grids around the data with appropriate weighting. +**Disadvantage** : Accuracy may be degraded due to extreme bias of the data. For example, if data $z(k)$ is biased near $Z_{RR}$ in Fig. 2, updating is performed at the four surrounding points ( $Z_{RR}$, $Z_{RL}$, $Z_{LR}$, and $Z_{LL}$), but accuracy at $Z_{LL}$ is not expected. + +

+ +

+ +#### Implementation + +See eq.(7)-(10) in [1] for the updated formula. In addition, eq.(17),(18) from [1] are used for Anti-Windup. + +### References + +[1] [Gabrielle Lochrie, Michael Doljevic, Mario Nona, Yongsoon Yoon, Anti-Windup Recursive Least Squares Method for Adaptive Lookup Tables with Application to Automotive Powertrain Control Systems, IFAC-PapersOnLine, Volume 54, Issue 20, 2021, Pages 840-845](https://www.sciencedirect.com/science/article/pii/S240589632102320X) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml index c3379ee67f7ca..69a65a8d540a2 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml @@ -8,6 +8,7 @@ + @@ -25,7 +26,9 @@ - + + + diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/media/fourcell_RLS.png b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/media/fourcell_RLS.png new file mode 100644 index 0000000000000..8929f3d64cdb3 Binary files /dev/null and b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/media/fourcell_RLS.png differ diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index ca65b332093a7..9b5cfc9186495 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -35,15 +35,43 @@ class DrawGraph(Node): def __init__(self): super().__init__("plot_server") + self.srv = self.create_service( CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback ) - package_path = get_package_share_directory("accel_brake_map_calibrator") default_map_path = get_package_share_directory("raw_vehicle_cmd_converter") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_default_map_dir", default_map_path + "/data/default/" + ) + self.default_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") + .get_parameter_value() + .string_value + ) + + package_path = get_package_share_directory("accel_brake_map_calibrator") + self.declare_parameter( + "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" + ) + self.calibrated_map_dir = ( + self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") + .get_parameter_value() + .string_value + ) + + self.declare_parameter("calibration_method", "each_cell") + self.calibration_method = ( + self.get_parameter("calibration_method").get_parameter_value().string_value + ) + if self.calibration_method is None: + self.calibration_method = "each_cell" + elif not ( + (self.calibration_method == "each_cell") | (self.calibration_method == "four_cell") + ): + print("invalid method.") + self.calibration_method = "each_cell" - self.default_map_dir = default_map_path + "/data/default/" - self.calibrated_map_dir = package_path + "/config/" self.log_file = package_path + "/config/log.csv" config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" @@ -71,6 +99,7 @@ def __init__(self): # debug self.get_logger().info("default map dir: {}".format(self.default_map_dir)) self.get_logger().info("calibrated map dir: {}".format(self.calibrated_map_dir)) + self.get_logger().info("calibrated method: {}".format(self.calibration_method)) self.get_logger().info("log file :{}".format(self.log_file)) self.get_logger().info("min_vel_thr : {}".format(self.min_vel_thr)) self.get_logger().info("vel_diff_thr : {}".format(self.vel_diff_thr)) @@ -82,7 +111,7 @@ def __init__(self): def get_data_callback(self, request, response): # read csv - # If log file doesn't exsist, return empty data + # If log file doesn't exist, return empty data if not Path(self.log_file).exists(): response.graph_image = [] self.get_logger().info("svg data is empty") @@ -123,6 +152,7 @@ def get_data_callback(self, request, response): self.vel_diff_thr, CF.PEDAL_LIST, self.pedal_diff_thr, + self.calibration_method, ) count_map, average_map, stddev_map = CalcUtils.create_stat_map(data)