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/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/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..8980f665d916d 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -43,6 +43,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 +90,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 +108,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(); @@ -199,9 +201,6 @@ bool AutonomousMode::isModeChangeAvailable() 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 +212,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/trajectory_follower/design/mpc_lateral_controller-design.md b/control/trajectory_follower/design/mpc_lateral_controller-design.md index ee79cc88f206b..0883d160cb89b 100644 --- a/control/trajectory_follower/design/mpc_lateral_controller-design.md +++ b/control/trajectory_follower/design/mpc_lateral_controller-design.md @@ -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/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index 8ae6dc5449fc3..c64c231144b80 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -400,7 +400,7 @@ 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 */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index d38312505fed7..8ed6d44b85258 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -97,6 +97,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; diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index 9f394ff9baa6f..f9f58deadd6bb 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -179,6 +179,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp( // */ TRAJECTORY_FOLLOWER_PUBLIC 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 control diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 050727c40a50d..ed6472700e14a 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -180,7 +180,7 @@ 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 @@ -217,6 +217,17 @@ void MPC::setReferenceTrajectory( } } + /* 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) { + trajectory_follower::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); diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index aaefa696d6494..b2e0ac18753a5 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -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"); @@ -302,7 +304,8 @@ void MpcLateralController::setTrajectory( 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); + 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); diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index e4270909cdf21..6e5b31d340e6b 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -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,6 +403,31 @@ 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::trajectory_follower::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 control diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp index 909133dcd65bd..6aa767289c1fd 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/trajectory_follower/test/test_mpc.cpp @@ -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 { @@ -175,7 +176,8 @@ 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 @@ -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; @@ -251,7 +254,8 @@ TEST_F(MPCTest, OsqpCalculate) 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 = @@ -282,7 +286,8 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) 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 = @@ -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; @@ -344,7 +350,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) 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 = diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index a6d98fa682d74..92d15da9e49e6 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/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/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 76b5da140bfaa..f3819f155edfe 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -13,6 +13,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/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,7 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: false + enable_differential_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] 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 index 6ed118001fa6a..5edecdde26a11 100644 --- 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 @@ -7,11 +7,6 @@ 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 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 index 520637abed775..dfc2b2e6ddb44 100644 --- 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 @@ -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/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 index b9de5cc4f2e13..71dc2ac600685 100644 --- 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 @@ -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/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 index bf40c334f6498..9708456df4fe7 100644 --- 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 @@ -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/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index ff3ebae90e847..2400b8b449a6a 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,6 +25,11 @@ + + + + + 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/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 6bfcffde71d60..4c3b418239a5c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp src/pointcloud_map_loader/pointcloud_map_loader_module.cpp src/pointcloud_map_loader/partial_map_loader_module.cpp + src/pointcloud_map_loader/differential_map_loader_module.cpp src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 1683fd472e74a..c8f5bed19bb3d 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -12,6 +12,7 @@ Currently, it supports the following two types: - Publish raw pointcloud map - Publish downsampled pointcloud map - Send partial pointcloud map loading via ROS 2 service +- Send differential pointcloud map loading via ROS 2 service #### Publish raw pointcloud map (ROS 2 topic) @@ -28,6 +29,13 @@ Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. +#### Send differential pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. + ### Parameters | Name | Type | Description | Default value | @@ -35,6 +43,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces @@ -42,6 +51,7 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map - `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map +- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..8f3ccbff00360 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,7 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: false + enable_differential_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp new file mode 100644 index 0000000000000..b42b919edb59d --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -0,0 +1,85 @@ +// 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. + +#include "differential_map_loader_module.hpp" + +DifferentialMapLoaderModule::DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) +{ + get_differential_pcd_maps_service_ = node->create_service( + "service/get_differential_pcd_map", + std::bind( + &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void DifferentialMapLoaderModule::differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, const std::vector & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + std::vector should_remove(static_cast(cached_ids.size()), true); + for (const auto & ele : all_pcd_file_metadata_dict_) { + std::string path = ele.first; + PCDFileMetadata metadata = ele.second; + + // assume that the map ID = map path (for now) + std::string map_id = path; + + // skip if the pcd file is not within the queried area + if (!isGridWithinQueriedArea(area, metadata)) continue; + + auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); + if (id_in_cached_list != cached_ids.end()) { + int index = id_in_cached_list - cached_ids.begin(); + should_remove[index] = false; + } else { + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = + loadPointCloudMapCellWithID(path, map_id); + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); + } + } + + for (int i = 0; i < static_cast(cached_ids.size()); ++i) { + if (should_remove[i]) { + response->ids_to_remove.push_back(cached_ids[i]); + } + } +} + +bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res) +{ + auto area = req->area; + std::vector cached_ids = req->cached_ids; + differentialAreaLoad(area, cached_ids, res); + res->header.frame_id = "map"; + return true; +} + +autoware_map_msgs::msg::PointCloudMapCellWithID +DifferentialMapLoaderModule::loadPointCloudMapCellWithID( + const std::string path, const std::string map_id) const +{ + sensor_msgs::msg::PointCloud2 pcd; + if (pcl::io::loadPCDFile(path, pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; + pointcloud_map_cell_with_id.pointcloud = pcd; + pointcloud_map_cell_with_id.cell_id = map_id; + return pointcloud_map_cell_with_id; +} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp new file mode 100644 index 0000000000000..5d6188c0b1a1f --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -0,0 +1,59 @@ +// 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 POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ + +#include "utils.hpp" + +#include + +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class DifferentialMapLoaderModule +{ + using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; + +public: + explicit DifferentialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + +private: + rclcpp::Logger logger_; + + std::map all_pcd_file_metadata_dict_; + rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; + + bool onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res); + void differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area_info, const std::vector & cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const std::string path, const std::string map_id) const; +}; + +#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index b42be7308b67b..ecbe481345381 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -52,6 +52,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); + bool enable_differential_load = declare_parameter("enable_differential_load"); if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; @@ -65,10 +66,18 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load) { + if (enable_partial_load | enable_differential_load) { pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); + } + + if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); } + + if (enable_differential_load) { + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict_); + } } std::vector PointCloudMapLoaderNode::getPcdPaths( diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index a7c289dd0f9a8..d321902131a81 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -15,6 +15,7 @@ #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#include "differential_map_loader_module.hpp" #include "partial_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" @@ -43,6 +44,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::unique_ptr pcd_map_loader_; std::unique_ptr downsampled_pcd_map_loader_; std::unique_ptr partial_map_loader_; + std::unique_ptr differential_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 9e66b40367518..81ed9af3e2aa2 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -190,7 +190,6 @@ class MapBasedPredictionNode : public rclcpp::Node const float path_probability, const ManeuverProbability & maneuver_probability, const Maneuver & maneuver, std::vector & reference_paths); std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); - PosePath resamplePath(const PosePath & base_path) const; void updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths); diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 98e7e125f78af..74742179a1577 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -5,6 +5,8 @@ 0.1.0 This package implements a map_based_prediction Yutaka Shimizu + Tomoya Kimura + Shunsuke Miura Apache License 2.0 ament_cmake diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c5159387787ed..82fe883f379ab 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -15,6 +15,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" #include +#include #include #include @@ -37,17 +38,6 @@ namespace map_based_prediction { -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(); -} - lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data) { lanelet::ConstLanelets lanelets; @@ -246,7 +236,6 @@ bool hasPotentialToReach( return false; } -} // namespace MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) @@ -364,7 +353,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt visualization_msgs::msg::MarkerArray debug_markers; for (const auto & object : in_objects->objects) { - std::string object_id = toHexString(object.object_id); + std::string object_id = tier4_autoware_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame @@ -463,6 +452,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + if (predicted_path.path.empty()) { + continue; + } predicted_path.confidence = ref_path.probability; predicted_paths.push_back(predicted_path); @@ -744,7 +736,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets - const std::string object_id = toHexString(object.object_id); + const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); if (objects_history_.count(object_id) != 0) { const std::vector & possible_lanelet = objects_history_.at(object_id).back().future_possible_lanelets; @@ -817,7 +809,7 @@ void MapBasedPredictionNode::updateObjectsHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - std::string object_id = toHexString(object.object_id); + std::string object_id = tier4_autoware_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); ObjectData single_object_data; @@ -904,7 +896,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( const double object_detected_time) { // Step1. Check if we have the object in the buffer - const std::string object_id = toHexString(object.object_id); + const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); if (objects_history_.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -1063,7 +1055,7 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - std::string object_id = toHexString(object.object_id); + std::string object_id = tier4_autoware_utils::toHexString(object.object_id); if (objects_history_.count(object_id) == 0) { return; } @@ -1183,8 +1175,7 @@ std::vector MapBasedPredictionNode::convertPathType( lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); for (const auto & lanelet_p : prev_lanelet.centerline()) { geometry_msgs::msg::Pose current_p; - current_p.position = - tier4_autoware_utils::createPoint(lanelet_p.x(), lanelet_p.y(), lanelet_p.z()); + current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p); const double lane_yaw = lanelet::utils::getLaneletAngle(prev_lanelet, current_p.position); current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); @@ -1195,8 +1186,7 @@ std::vector MapBasedPredictionNode::convertPathType( for (const auto & lanelet : path) { for (const auto & lanelet_p : lanelet.centerline()) { geometry_msgs::msg::Pose current_p; - current_p.position = - tier4_autoware_utils::createPoint(lanelet_p.x(), lanelet_p.y(), lanelet_p.z()); + current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p); const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, current_p.position); current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); @@ -1214,70 +1204,14 @@ std::vector MapBasedPredictionNode::convertPathType( } // Resample Path - const auto resampled_converted_path = resamplePath(converted_path); + const auto resampled_converted_path = + motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); converted_paths.push_back(resampled_converted_path); } return converted_paths; } -PosePath MapBasedPredictionNode::resamplePath(const PosePath & base_path) const -{ - std::vector base_s(base_path.size()); - std::vector base_x(base_path.size()); - std::vector base_y(base_path.size()); - std::vector base_z(base_path.size()); - for (size_t i = 0; i < base_path.size(); ++i) { - base_x.at(i) = base_path.at(i).position.x; - base_y.at(i) = base_path.at(i).position.y; - base_z.at(i) = base_path.at(i).position.z; - base_s.at(i) = motion_utils::calcSignedArcLength(base_path, 0, i); - } - const double base_path_len = base_s.back(); - - std::vector resampled_s; - for (double s = 0.0; s <= base_path_len; s += reference_path_resolution_) { - resampled_s.push_back(s); - } - - if (resampled_s.empty()) { - return base_path; - } - - // Insert End Point - const double epsilon = 0.01; - if (std::fabs(resampled_s.back() - base_path_len) < epsilon) { - resampled_s.back() = base_path_len; - } else { - resampled_s.push_back(base_path_len); - } - - if (resampled_s.size() < 2) { - return base_path; - } - - const auto resampled_x = interpolation::lerp(base_s, base_x, resampled_s); - const auto resampled_y = interpolation::lerp(base_s, base_y, resampled_s); - const auto resampled_z = interpolation::lerp(base_s, base_z, resampled_s); - - PosePath resampled_path(resampled_x.size()); - // Position - for (size_t i = 0; i < resampled_path.size(); ++i) { - resampled_path.at(i).position = - tier4_autoware_utils::createPoint(resampled_x.at(i), resampled_y.at(i), resampled_z.at(i)); - } - // Orientation - for (size_t i = 0; i < resampled_path.size() - 1; ++i) { - const auto curr_p = resampled_path.at(i).position; - const auto next_p = resampled_path.at(i + 1).position; - const double yaw = std::atan2(next_p.y - curr_p.y, next_p.x - curr_p.x); - resampled_path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - } - resampled_path.back().orientation = resampled_path.at(resampled_path.size() - 2).orientation; - - return resampled_path; -} - bool MapBasedPredictionNode::isDuplicated( const std::pair & target_lanelet, const LaneletsData & lanelets_data) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index d4306eee4431d..7b2e3fa0e3d04 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -139,8 +139,7 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( const TrackedObject & object, const PosePath & ref_paths) { if (ref_paths.size() < 2) { - const PredictedPath empty_path; - return empty_path; + return generateStraightPath(object); } return generatePolynomialPath(object, ref_paths); @@ -190,8 +189,7 @@ PredictedPath PathGenerator::generatePolynomialPath( const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { - const PredictedPath empty_path; - return empty_path; + return generateStraightPath(object); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 0d88a905394e7..3ef9c66db9665 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -45,6 +45,13 @@ boost::optional getTransformAnonymous( const std::string & target_frame_id, const rclcpp::Time & time) { try { + // check if the frames are ready + std::string errstr; // This argument prevents error msg from being displayed in the terminal. + if (!tf_buffer.canTransform( + target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return boost::none; + } + geometry_msgs::msg::TransformStamped self_transform_stamped; self_transform_stamped = tf_buffer.lookupTransform( /*target*/ target_frame_id, /*src*/ source_frame_id, time, 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/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 68cc14a8299a1..da2342f6782d4 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -9,12 +9,6 @@ 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 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 84c36bd093d04..75d1f798d8b65 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,12 +99,13 @@ 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_; @@ -161,27 +163,25 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); /** - * @brief extract path candidate from behavior tree output + * @brief skip smooth goal connection */ - PathWithLaneId::SharedPtr getPathCandidate( - const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); + bool skipSmoothGoalConnection( + const std::vector> & statuses) const; + + bool keepInputPoints(const std::vector> & statuses) const; /** * @brief skip smooth goal connection */ - bool skipSmoothGoalConnection( - const std::vector> & statuses) const; + void computeTurnSignal( + const std::shared_ptr planner_data, const PathWithLaneId & path, + const BehaviorModuleOutput & output); // debug rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_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 */ @@ -197,6 +197,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..8103092e23cdc 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; 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/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index d86461911a0c2..91b851ace15b4 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{}; }; @@ -130,7 +127,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 +225,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 +259,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/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index a6978c8e27d1a..3697ad5576b1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -288,7 +288,7 @@ std::vector generateDrivableLanes(const lanelet::ConstLanelets & std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); -size_t getOverlappedLaneletId(const std::vector & lanes); +boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); 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 b536689ed7b18..58afd67a77f14 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); @@ -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(); @@ -211,11 +219,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter("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); @@ -618,7 +621,11 @@ void BehaviorPathPlannerNode::run() // update planner data planner_data_->self_pose = self_pose_listener_.getCurrentPose(); - const auto planner_data = planner_data_; + const auto planner_data = std::make_shared(*planner_data_); + + // unlock planner data + mutex_pd_.unlock(); + // run behavior planner const auto output = bt_manager_->run(planner_data); @@ -627,7 +634,9 @@ void BehaviorPathPlannerNode::run() // update planner data planner_data_->prev_output_path = path; - mutex_pd_.unlock(); + + // compute turn signal + computeTurnSignal(planner_data, *path, output); // publish drivable bounds publish_bounds(*path); @@ -642,28 +651,7 @@ 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)); - - // for turn signal - { - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { - turn_signal.command = TurnIndicatorsCommand::DISABLE; - hazard_signal.command = output.turn_signal_info.hazard_signal.command; - } else { - turn_signal = - turn_signal_decider_.getTurnSignal(planner_data, *path, output.turn_signal_info); - hazard_signal.command = HazardLightsCommand::DISABLE; - } - turn_signal.stamp = get_clock()->now(); - hazard_signal.stamp = get_clock()->now(); - turn_signal_publisher_->publish(turn_signal); - hazard_signal_publisher_->publish(hazard_signal); - - publish_steering_factor(turn_signal); - } + publishPathCandidate(bt_manager_->getSceneModules()); publishSceneModuleDebugMsg(); @@ -677,6 +665,27 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } +void BehaviorPathPlannerNode::computeTurnSignal( + const std::shared_ptr planner_data, const PathWithLaneId & path, + const BehaviorModuleOutput & output) +{ + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + if (output.turn_signal_info.hazard_signal.command == HazardLightsCommand::ENABLE) { + turn_signal.command = TurnIndicatorsCommand::DISABLE; + hazard_signal.command = output.turn_signal_info.hazard_signal.command; + } else { + turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info); + hazard_signal.command = HazardLightsCommand::DISABLE; + } + turn_signal.stamp = get_clock()->now(); + hazard_signal.stamp = get_clock()->now(); + turn_signal_publisher_->publish(turn_signal); + hazard_signal_publisher_->publish(hazard_signal); + + publish_steering_factor(turn_signal); +} + void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) { const auto [intersection_flag, approaching_intersection_flag] = @@ -756,6 +765,39 @@ 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); + + 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) { @@ -775,52 +817,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"; 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..1f7979c9b726c 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 @@ -2219,6 +2219,8 @@ BehaviorModuleOutput AvoidanceModule::plan() { DEBUG_PRINT("AVOIDANCE plan"); + resetPathCandidate(); + const auto shift_lines = calcShiftLines(current_raw_shift_lines_, debug_data_); const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter_); @@ -2366,7 +2368,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; } @@ -2773,6 +2775,7 @@ void AvoidanceModule::initVariables() debug_data_ = DebugData(); debug_marker_.markers.clear(); + resetPathCandidate(); registered_raw_shift_lines_ = {}; current_raw_shift_lines_ = {}; original_unique_id = 0; 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..135c26d3dfe3a 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 @@ -143,6 +143,8 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { + resetPathCandidate(); + auto path = status_.lane_change_path.path; if (!isValidPath(path)) { @@ -201,7 +203,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() BehaviorModuleOutput out; out.path = std::make_shared(getReferencePath()); const auto candidate = planCandidate(); - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); waitApproval(); return out; @@ -640,6 +642,7 @@ void LaneChangeModule::resetParameters() 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/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 5ebbf2e415470..e89ad45cdadd9 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 @@ -100,6 +100,7 @@ void PullOutModule::onExit() clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); + resetPathCandidate(); current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit"); } @@ -165,6 +166,7 @@ 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); } @@ -295,7 +297,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) { 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..54b78686b1e2a 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 @@ -181,6 +181,7 @@ void PullOverModule::onExit() RCLCPP_DEBUG(getLogger(), "PULL_OVER onExit"); clearWaitingApproval(); removeRTCStatus(); + resetPathCandidate(); steering_factor_interface_ptr_->clearSteeringFactors(); // A child node must never return IDLE @@ -346,6 +347,8 @@ 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 = @@ -449,7 +452,7 @@ BehaviorModuleOutput PullOverModule::plan() // safe: use pull over path if (status_.is_safe) { output.path = std::make_shared(getCurrentPath()); - output.path_candidate = std::make_shared(getFullPath()); + path_candidate_ = std::make_shared(getFullPath()); } else { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path. Stop in road lane."); @@ -521,7 +524,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() BehaviorModuleOutput out; plan(); // update status_ out.path = std::make_shared(getReferencePath()); - out.path_candidate = status_.is_safe ? std::make_shared(getFullPath()) : out.path; + path_candidate_ = status_.is_safe ? std::make_shared(getFullPath()) : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); 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..116663ad3d797 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 BT::NodeStatus::FAILURE; } 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/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index b9289248465c0..500eb5f90c774 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -59,17 +59,10 @@ double calcInterpolatedVelocity( const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; return interpolated_vel; } -} // namespace - -namespace drivable_area_utils -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using route_handler::RouteHandler; template size_t findNearestSegmentIndex( - const std::vector & points, const Pose & pose, const double dist_threshold, + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { const auto nearest_idx = @@ -81,277 +74,43 @@ size_t findNearestSegmentIndex( return motion_utils::findNearestSegmentIndex(points, pose.position); } -double quantize(const double val, const double resolution) -{ - return std::round(val / resolution) * resolution; -} - -void updateMinMaxPosition( - const Eigen::Vector2d & point, boost::optional & min_x, boost::optional & min_y, - boost::optional & max_x, boost::optional & max_y) -{ - min_x = min_x ? std::min(min_x.get(), point.x()) : point.x(); - min_y = min_y ? std::min(min_y.get(), point.y()) : point.y(); - max_x = max_x ? std::max(max_x.get(), point.x()) : point.x(); - max_y = max_y ? std::max(max_y.get(), point.y()) : point.y(); -} - -bool sumLengthFromTwoPoints( - const Eigen::Vector2d & base_point, const Eigen::Vector2d & target_point, - const double length_threshold, double & sum_length, boost::optional & min_x, - boost::optional & min_y, boost::optional & max_x, boost::optional & max_y) -{ - const double norm_length = (base_point - target_point).norm(); - sum_length += norm_length; - if (length_threshold < sum_length) { - const double diff_length = norm_length - (sum_length - length_threshold); - const Eigen::Vector2d interpolated_point = - base_point + diff_length * (target_point - base_point).normalized(); - updateMinMaxPosition(interpolated_point, min_x, min_y, max_x, max_y); - - const bool is_end = true; - return is_end; - } - - updateMinMaxPosition(target_point, min_x, min_y, max_x, max_y); - const bool is_end = false; - return is_end; -} - -void fillYawFromXY(std::vector & points) -{ - if (points.size() < 2) { - return; - } - - for (size_t i = 0; i < points.size(); ++i) { - const size_t prev_idx = (i == points.size() - 1) ? i - 1 : i; - const size_t next_idx = (i == points.size() - 1) ? i : i + 1; - - const double yaw = tier4_autoware_utils::calcAzimuthAngle( - points.at(prev_idx).position, points.at(next_idx).position); - points.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - } -} - -lanelet::ConstLanelets extractLanesFromPathWithLaneId( - const std::shared_ptr & route_handler, const PathWithLaneId & path) -{ - // extract "unique" lane ids from path_with_lane_id - std::vector path_lane_ids; - for (const auto & path_point : path.points) { - for (const size_t lane_id : path_point.lane_ids) { - if (std::find(path_lane_ids.begin(), path_lane_ids.end(), lane_id) == path_lane_ids.end()) { - path_lane_ids.push_back(lane_id); - } +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Pose & pose) +{ + size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); + double min_lateral_dist = + std::fabs(motion_utils::calcLateralOffset(points, pose.position, 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); + 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) { + continue; } - } - - // get lanes according to lane ids - lanelet::ConstLanelets path_lanes; - path_lanes.reserve(path_lane_ids.size()); - for (const auto path_lane_id : path_lane_ids) { - const auto & lane = route_handler->getLaneletsFromId(static_cast(path_lane_id)); - path_lanes.push_back(lane); - } - - return path_lanes; -} -size_t getNearestLaneId(const lanelet::ConstLanelets & path_lanes, const Pose & current_pose) -{ - lanelet::ConstLanelet closest_lanelet; - if (lanelet::utils::query::getClosestLanelet(path_lanes, current_pose, &closest_lanelet)) { - for (size_t i = 0; i < path_lanes.size(); ++i) { - if (path_lanes.at(i).id() == closest_lanelet.id()) { - return i; - } + const double lat_dist = + std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; } } - return 0; -} - -void updateMinMaxPositionFromForwardLanelet( - const lanelet::ConstLanelets & path_lanes, const std::vector & points, - const Pose & current_pose, const double & forward_lane_length, const double & lane_margin, - const size_t & nearest_lane_idx, const size_t & nearest_segment_idx, - const std::function & - get_bound_func, - boost::optional & min_x, boost::optional & min_y, boost::optional & max_x, - boost::optional & max_y) -{ - const auto forward_offset_length = motion_utils::calcSignedArcLength( - points, current_pose.position, nearest_segment_idx, nearest_segment_idx); - double sum_length = std::min(forward_offset_length, 0.0); - size_t current_lane_idx = nearest_lane_idx; - auto current_lane = path_lanes.at(current_lane_idx); - size_t current_point_idx = nearest_segment_idx; - while (true) { - const auto & bound = get_bound_func(current_lane); - if (current_point_idx != bound.size() - 1) { - const Eigen::Vector2d & current_point = bound[current_point_idx].basicPoint(); - const Eigen::Vector2d & next_point = bound[current_point_idx + 1].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - current_point, next_point, forward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - ++current_point_idx; - } else { - const auto previous_lane = current_lane; - const size_t previous_point_idx = get_bound_func(previous_lane).size() - 1; - const auto & previous_bound = get_bound_func(previous_lane); - drivable_area_utils::updateMinMaxPosition( - previous_bound[previous_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - - if (current_lane_idx == path_lanes.size() - 1) { - break; - } - - current_lane_idx += 1; - current_lane = path_lanes.at(current_lane_idx); - current_point_idx = 0; - const auto & current_bound = get_bound_func(current_lane); - - const Eigen::Vector2d & prev_point = previous_bound[previous_point_idx].basicPoint(); - const Eigen::Vector2d & current_point = current_bound[current_point_idx].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - prev_point, current_point, forward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - } - } + return closest_idx; } -void updateMinMaxPositionFromBackwardLanelet( - const lanelet::ConstLanelets & path_lanes, const std::vector & points, - const Pose & current_pose, const double & backward_lane_length, const double & lane_margin, - const size_t & nearest_lane_idx, const size_t & nearest_segment_idx, - const std::function & - get_bound_func, - boost::optional & min_x, boost::optional & min_y, boost::optional & max_x, - boost::optional & max_y) -{ - size_t current_point_idx = nearest_segment_idx + 1; - const auto backward_offset_length = motion_utils::calcSignedArcLength( - points, nearest_segment_idx + 1, current_pose.position, nearest_segment_idx); - double sum_length = std::min(backward_offset_length, 0.0); - size_t current_lane_idx = nearest_lane_idx; - lanelet::ConstLanelet current_lane = path_lanes.at(current_lane_idx); - while (true) { - const auto & bound = get_bound_func(current_lane); - if (current_point_idx != 0) { - const Eigen::Vector2d & current_point = bound[current_point_idx].basicPoint(); - const Eigen::Vector2d & prev_point = bound[current_point_idx - 1].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - current_point, prev_point, backward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - - --current_point_idx; - } else { - const auto next_lane = current_lane; - const size_t next_point_idx = 0; - const auto & next_bound = get_bound_func(next_lane); - drivable_area_utils::updateMinMaxPosition( - next_bound[next_point_idx].basicPoint(), min_x, min_y, max_x, max_y); - - if (current_lane_idx == 0) { - break; - } - - current_lane_idx -= 1; - current_lane = path_lanes.at(current_lane_idx); - const auto & current_bound = get_bound_func(current_lane); - current_point_idx = current_bound.size() - 1; - - const Eigen::Vector2d & next_point = next_bound[next_point_idx].basicPoint(); - const Eigen::Vector2d & current_point = current_bound[current_point_idx].basicPoint(); - const bool is_end_lane = drivable_area_utils::sumLengthFromTwoPoints( - next_point, current_point, backward_lane_length + lane_margin, sum_length, min_x, min_y, - max_x, max_y); - if (is_end_lane) { - break; - } - } - } -} -std::array getPathScope( - const PathWithLaneId & path, const std::shared_ptr & route_handler, - const Pose & current_pose, const double forward_lane_length, const double backward_lane_length, - const double lane_margin, const double max_dist, const double max_yaw) +bool checkHasSameLane( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelet & target_lane) { - const lanelet::ConstLanelets path_lanes = extractLanesFromPathWithLaneId(route_handler, path); - - const size_t nearest_lane_idx = getNearestLaneId(path_lanes, current_pose); - - // define functions to get right/left bounds as a vector - const std::vector> - get_bound_funcs{ - [](const lanelet::ConstLanelet & lane) -> lanelet::ConstLineString2d { - return lane.rightBound2d(); - }, - [](const lanelet::ConstLanelet & lane) -> lanelet::ConstLineString2d { - return lane.leftBound2d(); - }}; - - // calculate min/max x and y - boost::optional min_x; - boost::optional min_y; - boost::optional max_x; - boost::optional max_y; - - for (const auto & get_bound_func : get_bound_funcs) { - // search nearest point index to current pose - const auto & nearest_bound = get_bound_func(path_lanes.at(nearest_lane_idx)); - if (nearest_bound.empty()) { - continue; - } - - const std::vector points = std::invoke([&nearest_bound]() { - std::vector points; - points.reserve(nearest_bound.size()); - for (const auto & point : nearest_bound) { // calculate x and y - Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - points.push_back(p); - } - - fillYawFromXY(points); // calculate yaw - return points; - }); - - const size_t nearest_segment_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, current_pose, max_dist, max_yaw); - - // forward lanelet - updateMinMaxPositionFromForwardLanelet( - path_lanes, points, current_pose, forward_lane_length, lane_margin, nearest_lane_idx, - nearest_segment_idx, get_bound_func, min_x, min_y, max_x, max_y); - - // backward lanelet - updateMinMaxPositionFromBackwardLanelet( - path_lanes, points, current_pose, backward_lane_length, lane_margin, nearest_lane_idx, - nearest_segment_idx, get_bound_func, min_x, min_y, max_x, max_y); - } - - if (!min_x || !min_y || !max_x || !max_y) { - const double x = current_pose.position.x; - const double y = current_pose.position.y; - return {x, y, x, y}; - } + if (lanelets.empty()) return false; - return {min_x.get(), min_y.get(), max_x.get(), max_y.get()}; + 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 drivable_area_utils +} // namespace namespace behavior_path_planner::util { @@ -1034,7 +793,7 @@ bool setGoal( // NOTE: goal does not have valid z, that will be calculated by interpolation here PathPointWithLaneId refined_goal{}; const size_t closest_seg_idx_for_goal = - drivable_area_utils::findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); + findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); refined_goal.point.pose = goal; refined_goal.point.pose.position.z = calcInterpolatedZ(input, goal.position, closest_seg_idx_for_goal); @@ -1046,8 +805,8 @@ bool setGoal( constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); - const size_t closest_seg_idx_for_pre_goal = drivable_area_utils::findNearestSegmentIndex( - input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); + const size_t closest_seg_idx_for_pre_goal = + findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = calcInterpolatedZ(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); pre_refined_goal.point.longitudinal_velocity_mps = @@ -1269,7 +1028,7 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } -size_t getOverlappedLaneletId(const std::vector & lanes) +boost::optional getOverlappedLaneletId(const std::vector & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { const auto lanelets = transformToLanelets(lanes); @@ -1277,7 +1036,7 @@ size_t getOverlappedLaneletId(const std::vector & lanes) for (const auto & lanelet : lanelets) { for (const auto & target_lanelet : target_lanelets) { - if (boost::geometry::overlaps( + if (boost::geometry::intersects( lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon())) { return true; } @@ -1289,7 +1048,7 @@ size_t getOverlappedLaneletId(const std::vector & lanes) }; if (lanes.size() <= 2) { - return lanes.size(); + return {}; } size_t overlapped_idx = lanes.size(); @@ -1307,15 +1066,15 @@ size_t getOverlappedLaneletId(const std::vector & lanes) std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes) { - const size_t overlapped_lanelet_id = getOverlappedLaneletId(lanes); - if (overlapped_lanelet_id == lanes.size()) { + const auto overlapped_lanelet_id = getOverlappedLaneletId(lanes); + if (!overlapped_lanelet_id) { return lanes; } const std::vector shorten_lanes{ - lanes.begin(), lanes.begin() + overlapped_lanelet_id}; + lanes.begin(), lanes.begin() + *overlapped_lanelet_id}; const std::vector removed_lanes{ - lanes.begin() + overlapped_lanelet_id, lanes.end()}; + lanes.begin() + *overlapped_lanelet_id, lanes.end()}; const auto transformed_lanes = util::transformToLanelets(removed_lanes); @@ -1344,16 +1103,6 @@ std::vector cutOverlappedLanes( return shorten_lanes; } -size_t findNearestSegmentIndex( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) -{ - const auto nearest_idx = - motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); - return nearest_idx ? nearest_idx.get() - : motion_utils::findNearestSegmentIndex(points, pose.position); -} - geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) @@ -1389,9 +1138,6 @@ void generateDrivableArea( const auto transformed_lanes = util::transformToLanelets(lanes); const auto current_pose = planner_data->self_pose; const auto route_handler = planner_data->route_handler; - const auto & param = planner_data->parameters; - const double dist_threshold = std::numeric_limits::max(); - const double yaw_threshold = param.ego_nearest_yaw_threshold; constexpr double overlap_threshold = 0.01; auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { @@ -1427,10 +1173,41 @@ void generateDrivableArea( addRightBoundPoints(lane.right_lane); } + 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 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 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); } @@ -1444,38 +1221,36 @@ void generateDrivableArea( 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 = - findNearestSegmentIndex(left_bound, front_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose); const size_t front_right_start_idx = - findNearestSegmentIndex(right_bound, front_pose, dist_threshold, yaw_threshold); + 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); const size_t left_start_idx = - findNearestSegmentIndex(left_bound, left_start_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_pose); const size_t right_start_idx = - findNearestSegmentIndex(right_bound, right_start_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_pose); // 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 = - findNearestSegmentIndex(left_bound, goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose); const size_t goal_right_start_idx = - findNearestSegmentIndex(right_bound, goal_pose, dist_threshold, yaw_threshold); + 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); const size_t left_goal_idx = - findNearestSegmentIndex(left_bound, left_goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_pose); const size_t right_goal_idx = - findNearestSegmentIndex(right_bound, right_goal_pose, dist_threshold, yaw_threshold); + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_pose); // Store Data path.left_bound.clear(); path.right_bound.clear(); - path.left_bound.reserve(left_goal_idx - left_start_idx); - path.right_bound.reserve(right_goal_idx - right_start_idx); // Insert a start point path.left_bound.push_back(left_start_pose.position); 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..2dabbdc537b99 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; 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/README.md b/planning/mission_planner/README.md index 028352362a0ba..388f35e770359 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -14,12 +14,13 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Parameters -| Name | Type | Description | -| ------------------------- | ------ | --------------------------------- | -| `map_frame` | string | The frame name for map | -| `arrival_check_angle_deg` | double | Angle threshold for goal check | -| `arrival_check_distance` | double | Distance threshold for goal check | -| `arrival_check_duration` | double | Duration threshold for goal check | +| Name | Type | Description | +| ------------------------- | ------ | ------------------------------------ | +| `map_frame` | string | The frame name for map | +| `arrival_check_angle_deg` | double | Angle threshold for goal check | +| `arrival_check_distance` | double | Distance threshold for goal check | +| `arrival_check_duration` | double | Duration threshold for goal check | +| `goal_angle_threshold` | double | Max goal pose angle for goal approve | ### Services @@ -38,11 +39,12 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Publications -| Name | Type | Description | -| ------------------------------- | --------------------------------------- | ---------------------- | -| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | -| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | -| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | +| Name | Type | Description | +| ------------------------------- | --------------------------------------- | ------------------------ | +| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | +| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | +| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | +| `debug/goal_footprint` | visualization_msgs::msg::MarkerArray | goal footprint for debug | ## Route section @@ -57,6 +59,15 @@ The ROS message of route section contains following three elements for each rout - `preferred_primitive`: Preferred lane to follow towards the goal. - `primitives`: All neighbor lanes in the same direction including the preferred lane. +## Goal Validation + +The mission planner has control mechanism to validate the given goal pose and create a route. If goal pose angle between goal pose lanelet and goal pose' yaw is greater than `goal_angle_threshold` parameter, the goal is rejected. +Another control mechanism is the creation of a footprint of the goal pose according to the dimensions of the vehicle and checking whether this footprint is within the lanelets. If goal footprint exceeds lanelets, then the goal is rejected. + +At the image below, there are sample goal pose validation cases. + +![goal_footprints](./media/goal_footprints.svg) + ## Implementation ### Mission Planner diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml index 20c2cfbd2a0b7..d3a36f1e5cf6d 100644 --- a/planning/mission_planner/launch/mission_planner.launch.xml +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -8,6 +8,7 @@ + diff --git a/planning/mission_planner/media/goal_footprints.svg b/planning/mission_planner/media/goal_footprints.svg new file mode 100644 index 0000000000000..c3347c0b52410 --- /dev/null +++ b/planning/mission_planner/media/goal_footprints.svg @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Goal Pose is Valid +
+
+
+ + Goal Pose... + + + + + + +
+
+
+ Goal Pose is not Valid, goal footprint exceeds lane boundaries +
+
+
+ + Goal Pose is not Valid, go... + + + + + + +
+
+
+ Goal Pose is not Valid, goal footprint exceeds opposite lane +
+
+
+ + Goal Pose is not Valid, go... + + + + + + +
+
+
+ Goal Pose is Valid +
+
+
+ + Goal Pose... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 9e5764dfca638..f972c18b66d0d 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -31,6 +31,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index e02d055e20577..b741bd4479763 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -113,6 +114,13 @@ void DefaultPlanner::initialize(rclcpp::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_ = + node_->create_publisher("debug/goal_footprint", durable_qos); + + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg", 45.0); } void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) @@ -186,12 +194,110 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) return route_marker_array; } -bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const +visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( + tier4_autoware_utils::LinearRing2d goal_footprint) const +{ + visualization_msgs::msg::MarkerArray msg; + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock().now(), "goal_footprint", 0, visualization_msgs::msg::Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.99, 0.99, 0.2, 1.0)); + marker.lifetime = rclcpp::Duration::from_seconds(2.5); + + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[0][0], goal_footprint[0][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[1][0], goal_footprint[1][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[2][0], goal_footprint[2][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[3][0], goal_footprint[3][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[4][0], goal_footprint[4][1], 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(goal_footprint[5][0], goal_footprint[5][1], 0.0)); + marker.points.push_back(marker.points.front()); + + msg.markers.push_back(marker); + + return msg; +} + +bool DefaultPlanner::check_goal_footprint( + const lanelet::ConstLanelet & current_lanelet, + const lanelet::ConstLanelet & combined_prev_lanelet, + const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const double search_margin) +{ + std::vector points_intersection; + + // check if goal footprint is in current lane + boost::geometry::intersection( + goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon(), points_intersection); + if (points_intersection.empty()) { + return true; + } + points_intersection.clear(); + + // check if goal footprint is in between many lanelets + for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) { + next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); + lanelet::ConstLanelets lanelets; + lanelets.push_back(combined_prev_lanelet); + lanelets.push_back(next_lane); + lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + + // if next lanelet length longer than vehicle longitudinal offset + if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { + next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); + boost::geometry::intersection( + goal_footprint, combined_lanelets.polygon2d().basicPolygon(), points_intersection); + if (points_intersection.empty()) { + return true; + } + points_intersection.clear(); + } else { // if next lanelet length shorter than vehicle longitudinal offset -> recursive call + if (!check_goal_footprint(next_lane, combined_lanelets, goal_footprint, next_lane_length)) { + next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); + continue; + } else { + return true; + } + } + } + return false; +} + +bool DefaultPlanner::is_goal_valid( + const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { + const auto logger = node_->get_logger(); lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; } + + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + tier4_autoware_utils::LinearRing2d goal_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal)); + pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); + const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); + + double next_lane_length = 0.0; + // combine calculated route lanelets + lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + + // check if goal footprint exceeds lane when the goal isn't in parking_lot + if ( + !check_goal_footprint( + closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && + !is_in_parking_lot( + lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_), + lanelet::utils::conversion::toLaneletPoint(goal.position))) { + RCLCPP_WARN(logger, "Goal's footprint exceeds lane!"); + return false; + } + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { @@ -199,8 +305,7 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const const auto goal_yaw = tf2::getYaw(goal.orientation); const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; - + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } @@ -230,12 +335,11 @@ bool DefaultPlanner::is_goal_valid(const geometry_msgs::msg::Pose & goal) const const auto goal_yaw = tf2::getYaw(goal.orientation); const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - constexpr double th_angle = M_PI / 4; + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); if (std::abs(angle_diff) < th_angle) { return true; } } - return false; } @@ -255,11 +359,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) LaneletRoute route_msg; RouteSections route_sections; - if (!is_goal_valid(points.back())) { - RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose"); - return route_msg; - } - + lanelet::ConstLanelets all_route_lanelets; for (std::size_t i = 1; i < points.size(); i++) { const auto start_check_point = points.at(i - 1); const auto goal_check_point = points.at(i); @@ -268,12 +368,20 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) start_check_point, goal_check_point, &path_lanelets)) { return route_msg; } + for (const auto & lane : path_lanelets) { + all_route_lanelets.push_back(lane); + } // create local route sections route_handler_.setRouteLanelets(path_lanelets); const auto local_route_sections = route_handler_.createMapSegments(path_lanelets); route_sections = combine_consecutive_route_sections(route_sections, local_route_sections); } + if (!is_goal_valid(points.back(), all_route_lanelets)) { + RCLCPP_WARN(logger, "Goal is not valid! Please check position and angle of goal_pose"); + return route_msg; + } + if (route_handler_.isRouteLooped(route_sections)) { RCLCPP_WARN(logger, "Loop detected within route!"); return route_msg; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index e532261e73e1f..20f731953667a 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -32,6 +33,11 @@ namespace mission_planner::lanelet2 { +struct DefaultPlannerParameters +{ + double goal_angle_threshold_deg; +}; + class DefaultPlanner : public mission_planner::PlannerPlugin { public: @@ -40,6 +46,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; MarkerArray visualize(const LaneletRoute & route) const override; + MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; + vehicle_info_util::VehicleInfo vehicle_info_; private: using RouteSections = std::vector; @@ -52,11 +60,19 @@ class DefaultPlanner : public mission_planner::PlannerPlugin lanelet::ConstLanelets shoulder_lanelets_; route_handler::RouteHandler route_handler_; + DefaultPlannerParameters param_; + rclcpp::Node * node_; rclcpp::Subscription::SharedPtr map_subscriber_; + rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; void map_callback(const HADMapBin::ConstSharedPtr msg); - bool is_goal_valid(const geometry_msgs::msg::Pose & goal) const; + bool check_goal_footprint( + const lanelet::ConstLanelet & current_lanelet, + const lanelet::ConstLanelet & combined_prev_lanelet, + const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length, + const double search_margin = 2.0); + bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets); Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections); }; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index d31d9ec3c0623..eaadd019b462c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -14,15 +14,32 @@ #include "utility_functions.hpp" +#include + #include #include +#include bool exists(const std::unordered_set & set, const lanelet::Id & id) { return set.find(id) != set.end(); } +tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( + tier4_autoware_utils::LinearRing2d footprint) +{ + tier4_autoware_utils::Polygon2d footprint_polygon; + boost::geometry::append(footprint_polygon.outer(), footprint[0]); + boost::geometry::append(footprint_polygon.outer(), footprint[1]); + boost::geometry::append(footprint_polygon.outer(), footprint[2]); + boost::geometry::append(footprint_polygon.outer(), footprint[3]); + boost::geometry::append(footprint_polygon.outer(), footprint[4]); + boost::geometry::append(footprint_polygon.outer(), footprint[5]); + boost::geometry::correct(footprint_polygon); + return footprint_polygon; +} + void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { cl->r = r; @@ -36,3 +53,35 @@ void insert_marker_array( { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } + +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + lanelet::Points3d centers; + std::vector 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()); + } + } + + for (const auto & llt : lanelets) { + if (std::count(bound_ids.begin(), bound_ids.end(), llt.leftBound().id()) < 2) { + 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) { + for (const auto & pt : llt.rightBound()) { + rights.push_back(lanelet::Point3d(pt)); + } + } + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return std::move(combined_lanelet); +} diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 8217502225439..2d986ac8e56ed 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,7 +15,11 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + #include +#include +#include #include #include @@ -41,8 +45,11 @@ bool exists(const std::vector & vectors, const T & item) return false; } +tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon( + tier4_autoware_utils::LinearRing2d footprint); void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a); void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); +lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 4d114c47bfaf0..5f372a5baed34 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -56,15 +56,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; }; ``` 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..43949c55b21a5 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 @@ -58,7 +58,6 @@ 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); @@ -72,11 +71,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; 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..8ce9ee3d7e925 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -100,7 +100,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); @@ -128,6 +129,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node 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 @@ -141,10 +144,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node 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}; + geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_{nullptr}; + geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; @@ -160,7 +162,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}; @@ -195,8 +197,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool need_to_clear_vel_limit_{false}; bool is_driving_forward_{true}; + bool disable_stop_planning_{false}; std::vector prev_target_obstacles_; + + std::shared_ptr lpf_acc_ptr_; }; } // namespace motion_planning 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/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index a6ed9d3e66bdf..c2308adc1ca17 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,83 @@ 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}; + }; + 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_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..a01d3589245b5 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,6 +17,7 @@ #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/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -38,6 +39,7 @@ 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_debug_msgs::msg::Float32MultiArrayStamped; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; @@ -105,6 +107,16 @@ class PlannerInterface smoothed_trajectory_ptr_ = 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 + { + return Float32MultiArrayStamped{}; + } + protected: // Parameters bool is_showing_debug_info_{false}; @@ -123,6 +135,9 @@ class PlannerInterface EgoNearestParam ego_nearest_param_; + // debug info + StopPlanningDebugInfo stop_planning_debug_info_; + // TODO(shimizu) remove these parameters Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; 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/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 39a4dd5de0815..71ec3e5ef200c 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -134,13 +134,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); @@ -192,6 +192,9 @@ Trajectory extendTrajectory( namespace motion_planning { +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), self_pose_listener_(this), @@ -223,11 +226,13 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & clear_vel_limit_pub_ = create_publisher( "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); 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); + 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); // longitudinal_info const auto longitudinal_info = [&]() { @@ -248,6 +253,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & const double terminal_safe_distance_margin = declare_parameter("common.terminal_safe_distance_margin"); + lpf_acc_ptr_ = std::make_shared(0.2); + return LongitudinalInfo{ max_accel, min_accel, @@ -273,6 +280,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & }(); 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 = @@ -424,7 +432,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); } } - // wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -456,6 +463,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + tier4_autoware_utils::updateParam( + parameters, "common.disable_stop_planning", disable_stop_planning_); + // obstacle_filtering tier4_autoware_utils::updateParam( parameters, "obstacle_filtering.rough_detection_area_expand_width", @@ -572,7 +582,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__); @@ -610,9 +621,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); } } @@ -651,7 +663,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); } } @@ -1024,11 +1036,11 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( } } -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; + MarkerArray debug_marker; const auto current_time = now(); // obstacles to cruise @@ -1055,7 +1067,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", current_time, "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 +1089,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", current_time, "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,7 +1099,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c debug_marker_pub_->publish(debug_marker); - // wall for cruise and stop + // 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); @@ -1098,6 +1110,19 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c __func__, calculation_time); } +void ObstacleCruisePlannerNode::publishDebugInfo() const +{ + const auto current_time = now(); + + // stop + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(current_time); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // cruise + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(current_time); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); +} + void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const { Float32Stamped calculation_time_msg; @@ -1106,5 +1131,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/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 267ff1f4502f2..cfd9dd24c543b 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,86 @@ 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"); + } - 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_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); } Trajectory PIDBasedPlanner::generateCruiseTrajectory( @@ -91,36 +148,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 +185,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,48 +290,68 @@ 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 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_data.cruise_wall_marker); + + // cruise obstalce + 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_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; }(); @@ -217,42 +360,251 @@ VelocityLimit PIDBasedPlanner::doCruise( 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 = [&]() { + 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; + }(); + + /* + 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); + */ + 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, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_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 = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + acc_limited_traj.points, start_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + 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 +612,79 @@ 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); + } - // 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..adee76f67c065 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -106,6 +106,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); @@ -236,6 +242,16 @@ Trajectory PlannerInterface::generateStopTrajectory( stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); } + // stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, // + // 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; } diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index dfd9a9dec757a..8845bbb52c056 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -28,9 +28,6 @@ import numpy as np import rclpy from rclpy.node import Node -import tf2_ros -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener from tier4_planning_msgs.msg import VelocityLimit parser = argparse.ArgumentParser() @@ -62,9 +59,6 @@ PLOT_TYPE = "VEL" print("plot type = " + PLOT_TYPE) -PATH_ORIGIN_FRAME = "map" -SELF_POSE_FRAME = "base_link" - class TrajectoryVisualizer(Node): def __init__(self): @@ -83,6 +77,7 @@ def __init__(self): # update flag self.update_ex_vel_lim = False self.update_lat_acc_fil = False + self.update_steer_rate_fil = False self.update_traj_raw = False self.update_traj_resample = False self.update_traj_final = False @@ -91,9 +86,6 @@ def __init__(self): self.update_traj_ob_avoid = False self.update_traj_ob_stop = False - self.tf_buffer = Buffer(node=self) - self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) - self.self_pose = Pose() self.self_pose_received = False self.localization_vx = 0.0 @@ -102,6 +94,7 @@ def __init__(self): self.trajectory_external_velocity_limited = Trajectory() self.trajectory_lateral_acc_filtered = Trajectory() + self.trajectory_steer_rate_filtered = Trajectory() self.trajectory_raw = Trajectory() self.trajectory_time_resampled = Trajectory() self.trajectory_final = Trajectory() @@ -112,7 +105,7 @@ def __init__(self): self.obstacle_stop_traj = Trajectory() self.plotted = [False] * 9 - self.sub_localization_twist = self.create_subscription( + self.sub_localization_kinematics = self.create_subscription( Odometry, "/localization/kinematic_state", self.CallbackLocalizationTwist, 1 ) self.sub_vehicle_twist = self.create_subscription( @@ -135,6 +128,9 @@ def __init__(self): self.sub4 = message_filters.Subscriber( self, Trajectory, optimizer_debug + "trajectory_time_resampled" ) + self.sub41 = message_filters.Subscriber( + self, Trajectory, optimizer_debug + "trajectory_steering_rate_limited" + ) self.sub5 = message_filters.Subscriber( self, Trajectory, "/planning/scenario_planning/trajectory" ) @@ -152,7 +148,7 @@ def __init__(self): self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") self.ts1 = message_filters.ApproximateTimeSynchronizer( - [self.sub1, self.sub2, self.sub3, self.sub4], 30, 0.5 + [self.sub1, self.sub2, self.sub3, self.sub4, self.sub41], 30, 0.5 ) self.ts1.registerCallback(self.CallbackMotionVelOptTraj) @@ -177,11 +173,10 @@ def __init__(self): return - def test(self): - self.updatePose("map", "base_link") - def CallbackLocalizationTwist(self, cmd): self.localization_vx = cmd.twist.twist.linear.x + self.self_pose = cmd.pose.pose + self.self_pose_received = True def CallbackVehicleTwist(self, cmd): self.vehicle_vx = cmd.longitudinal_velocity @@ -189,12 +184,13 @@ def CallbackVehicleTwist(self, cmd): def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity - def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4): + def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): print("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) self.CallBackTrajTimeResampled(cmd4) + self.CallBackTrajSteerRateFiltered(cmd5) def CallBackTrajExVelLim(self, cmd): self.trajectory_external_velocity_limited = cmd @@ -204,6 +200,10 @@ def CallBackTrajLatAccFiltered(self, cmd): self.trajectory_lateral_acc_filtered = cmd self.update_lat_acc_fil = True + def CallBackTrajSteerRateFiltered(self, cmd): + self.trajectory_steer_rate_filtered = cmd + self.update_steer_rate_fil = True + def CallBackTrajRaw(self, cmd): self.trajectory_raw = cmd self.update_traj_raw = True @@ -252,9 +252,12 @@ def setPlotTrajectoryVelocity(self): (self.im6,) = self.ax1.plot( [], [], label="4-2: opt external_velocity_limited", marker="", ls="--" ) - (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker="", ls="--") - (self.im8,) = self.ax1.plot([], [], label="4-4: opt time_resampled", marker="", ls="--") - (self.im9,) = self.ax1.plot([], [], label="4-5: opt final", marker="", ls="-") + (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker=".", ls="--") + (self.im71,) = self.ax1.plot( + [], [], label="4-4: opt steer_rate_filtered", marker="", ls="-." + ) + (self.im8,) = self.ax1.plot([], [], label="4-5: opt time_resampled", marker="*", ls="--") + (self.im9,) = self.ax1.plot([], [], label="4-6: opt final", marker="", ls="-") (self.im10,) = self.ax1.plot( [], [], label="localization twist vx", color="r", marker="*", ls=":", markersize=10 ) @@ -277,6 +280,7 @@ def setPlotTrajectoryVelocity(self): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -285,7 +289,7 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: print("plot start but self pose is not received") return ( @@ -296,6 +300,7 @@ def plotTrajectoryVelocity(self, data): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -312,6 +317,7 @@ def plotTrajectoryVelocity(self, data): trajectory_raw = self.trajectory_raw trajectory_external_velocity_limited = self.trajectory_external_velocity_limited trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered + trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered trajectory_time_resampled = self.trajectory_time_resampled trajectory_final = self.trajectory_final @@ -360,6 +366,12 @@ def plotTrajectoryVelocity(self, data): self.im7.set_data(x, y) self.update_lat_acc_fil = False + if self.update_steer_rate_fil: + x = self.CalcArcLength(trajectory_steer_rate_filtered) + y = self.ToVelList(trajectory_steer_rate_filtered) + self.im71.set_data(x, y) + self.update_steer_rate_fil = False + if self.update_traj_resample: x = self.CalcArcLength(trajectory_time_resampled) y = self.ToVelList(trajectory_time_resampled) @@ -391,6 +403,7 @@ def plotTrajectoryVelocity(self, data): self.im5, self.im6, self.im7, + self.im71, self.im8, self.im9, self.im10, @@ -577,7 +590,7 @@ def setPlotTrajectory(self): def plotTrajectory(self, data): print("plot called") - self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) # copy trajectory_raw = self.trajectory_raw @@ -668,25 +681,6 @@ def calcSquaredDist2d(self, p1, p2): dy = p1.position.y - p2.position.y return dx * dx + dy * dy - def updatePose(self, from_link, to_link): - try: - tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time()) - self.self_pose.position.x = tf.transform.translation.x - self.self_pose.position.y = tf.transform.translation.y - self.self_pose.position.z = tf.transform.translation.z - self.self_pose.orientation.x = tf.transform.rotation.x - self.self_pose.orientation.y = tf.transform.rotation.y - self.self_pose.orientation.z = tf.transform.rotation.z - self.self_pose.orientation.w = tf.transform.rotation.w - print("updatePose succeeded") - self.self_pose_received = True - return - except tf2_ros.TransformException: - self.get_logger().warn( - "lookup transform failed: from {} to {}".format(from_link, to_link) - ) - return - def closeFigure(self): plt.close(self.fig) 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/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..f75ceed95f294 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,14 @@ 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); + current_acc_ = (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 || @@ -75,31 +83,15 @@ 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(); } } 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