From 8b2eeaae6fc254fe4ebf96ff880eb8a4ae6d2c97 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 25 May 2023 12:08:14 +0900 Subject: [PATCH 1/9] feat(behavior_path_planner): output stop reasons (#3807) * feat(launch): remap stop reasons Signed-off-by: satoshi-ota * feat(behavior_path_planner): add interface to output stop reasons Signed-off-by: satoshi-ota * feat(behavior_path_planner): add interface to output stop reasons Signed-off-by: satoshi-ota * feat(avoidance): output stop reason Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_planning.launch.py | 1 + .../behavior_path_planner_node.hpp | 3 + .../behavior_path_planner/planner_manager.hpp | 34 +++++++++++- .../scene_module/scene_module_interface.hpp | 55 +++++++++++++++++++ .../src/behavior_path_planner_node.cpp | 2 + .../src/behavior_tree_manager.cpp | 1 + .../avoidance/avoidance_module.cpp | 2 + 7 files changed, 96 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 87481744fdaa4..affe9ac6af3e0 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"), + ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), ], parameters=[ common_param, 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 29ed73b9374e6..a69e6dde1c364 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 @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -90,6 +91,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::Scenario; +using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -114,6 +116,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr hazard_signal_publisher_; rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; + rclcpp::Publisher::SharedPtr stop_reason_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 6a12c6f7a93db..81f555dee77c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -37,6 +38,7 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; @@ -150,6 +152,34 @@ class PlannerManager return ret; } + /** + * @brief aggregate launched module's stop reasons. + * @return stop reason array + */ + StopReasonArray getStopReasons() const + { + StopReasonArray stop_reason_array; + stop_reason_array.header.frame_id = "map"; + stop_reason_array.header.stamp = clock_.now(); + + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + const auto reason = m->getStopReason(); + if (reason.reason != "") { + stop_reason_array.stop_reasons.push_back(m->getStopReason()); + } + }); + + std::for_each( + candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [&](const auto & m) { + const auto reason = m->getStopReason(); + if (reason.reason != "") { + stop_reason_array.stop_reasons.push_back(m->getStopReason()); + } + }); + + return stop_reason_array; + } + /** * @brief reset root lanelet. if there are approved modules, don't reset root lanelet. * @param planner data. @@ -362,9 +392,9 @@ class PlannerManager std::vector candidate_module_ptrs_; - rclcpp::Logger logger_; + mutable rclcpp::Logger logger_; - rclcpp::Clock clock_; + mutable rclcpp::Clock clock_; mutable StopWatch stop_watch_; 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 85b2f80cb2093..65b42d0242b54 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 @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -53,6 +56,9 @@ using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; using PlanResult = PathWithLaneId::SharedPtr; @@ -92,6 +98,11 @@ class SceneModuleInterface const auto ns = std::string("~/virtual_wall/") + utils::convertToSnakeCase(name); pub_virtual_wall_ = node.create_publisher(ns, 20); } + + { + const auto ns = std::string("~/output/stop_reasons"); + pub_stop_reasons_ = node.create_publisher(ns, 20); + } #endif for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -203,6 +214,8 @@ class SceneModuleInterface current_state_ = ModuleStatus::IDLE; #endif + stop_reason_ = StopReason(); + processOnEntry(); } @@ -221,6 +234,8 @@ class SceneModuleInterface unlockNewModuleLaunch(); steering_factor_interface_ptr_->clearSteeringFactors(); + stop_reason_ = StopReason(); + processOnExit(); } @@ -300,6 +315,22 @@ class SceneModuleInterface void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } + void publishStopReasons() + { + StopReasonArray stop_reason_array; + stop_reason_array.header.frame_id = "map"; + stop_reason_array.header.stamp = clock_->now(); + + const auto reason = getStopReason(); + if (reason.reason == "") { + return; + } + + stop_reason_array.stop_reasons.push_back(reason); + + pub_stop_reasons_->publish(stop_reason_array); + } + void publishVirtualWall() { MarkerArray markers{}; @@ -349,6 +380,8 @@ class SceneModuleInterface ModuleStatus getCurrentStatus() const { return current_state_; } + StopReason getStopReason() const { return stop_reason_; } + virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; std::string name() const { return name_; } @@ -421,6 +454,7 @@ class SceneModuleInterface rclcpp::Publisher::SharedPtr pub_info_marker_; rclcpp::Publisher::SharedPtr pub_debug_marker_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Publisher::SharedPtr pub_stop_reasons_; #endif BehaviorModuleOutput previous_module_output_; @@ -461,6 +495,22 @@ class SceneModuleInterface } } + void setStopReason(const std::string & stop_reason, const PathWithLaneId & path) + { + stop_reason_.reason = stop_reason; + + if (!stop_pose_) { + stop_reason_.reason = ""; + return; + } + + StopFactor stop_factor; + stop_factor.stop_pose = stop_pose_.get(); + stop_factor.dist_to_stop_pose = + motion_utils::calcSignedArcLength(path.points, getEgoPosition(), stop_pose_.get().position); + stop_reason_.stop_factors.push_back(stop_factor); + } + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } @@ -475,11 +525,14 @@ class SceneModuleInterface { return planner_data_->self_odometry->pose.pose.position; } + geometry_msgs::msg::Pose getEgoPose() const { return planner_data_->self_odometry->pose.pose; } + geometry_msgs::msg::Twist getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } + double getEgoSpeed() const { return std::abs(planner_data_->self_odometry->twist.twist.linear.x); @@ -518,6 +571,8 @@ class SceneModuleInterface ModuleStatus current_state_{ModuleStatus::IDLE}; #endif + StopReason stop_reason_; + std::unordered_map> rtc_interface_ptr_map_; std::unique_ptr steering_factor_interface_ptr_; 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 51650803f8be8..269e50dfa5182 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -67,6 +67,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); + stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); debug_lane_change_msg_array_publisher_ = @@ -1249,6 +1250,7 @@ void BehaviorPathPlannerNode::run() #else publishPathCandidate(planner_manager_->getSceneModuleManagers(), planner_data_); publishPathReference(planner_manager_->getSceneModuleManagers(), planner_data_); + stop_reason_publisher_->publish(planner_manager_->getStopReasons()); #endif #ifdef USE_OLD_ARCHITECTURE diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index c72f5b89ba36d..2e8627b745db8 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -115,6 +115,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { m->publishInfoMarker(); m->publishDebugMarker(); + m->publishStopReasons(); m->publishVirtualWall(); if (!m->isExecutionRequested()) { m->onExit(); 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 b21112d50cfc4..465a5c4764732 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 @@ -572,6 +572,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif default: throw std::domain_error("invalid behavior"); } + + setStopReason(StopReason::AVOIDANCE, path.path); } /** From c1de8fde953a996dcfa655dc6ddce196f3fcade6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 25 May 2023 12:47:33 +0900 Subject: [PATCH 2/9] fix(behavior_path_planner): fix keeping terminal point (#3813) --- planning/behavior_path_planner/src/utils/path_utils.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 5a5c1135d4c58..1b42c3f9d39dd 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -101,14 +101,18 @@ PathWithLaneId resamplePathWithSpline( const auto start_s = std::max(target_section.first, 0.0); const auto end_s = std::min(target_section.second, s_vec.back()); - for (double s = start_s; s < end_s - epsilon; s += interval) { + for (double s = start_s; s < end_s; s += interval) { if (!has_almost_same_value(s_out, s)) { s_out.push_back(s); } } // Insert Terminal Point - s_out.push_back(end_s); + if (!has_almost_same_value(s_out, end_s)) { + s_out.push_back(end_s); + } else { + s_out.back() = end_s; + } // Insert Stop Point const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path); From 2ea192ba230140d79af813cf9de02eb0eccc7f4f Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 25 May 2023 15:21:48 +0900 Subject: [PATCH 3/9] build(iron): remove rmw_qos_profile_t (#3809) Signed-off-by: wep21 --- .../src/bag_time_manager_panel.cpp | 15 ++------- .../src/rtc_manager_panel.cpp | 8 ++--- .../src/automatic_goal_sender.cpp | 14 ++++---- .../src/tools/manual_controller.cpp | 3 +- .../src/autoware_state_panel.cpp | 32 +++++++++---------- .../joy_controller/joy_controller_node.cpp | 4 +-- .../src/map_update_module.cpp | 4 +-- .../src/rtc_auto_mode_manager_interface.cpp | 4 +-- .../rtc_replayer/src/rtc_replayer_node.cpp | 3 +- 9 files changed, 36 insertions(+), 51 deletions(-) diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp index c2ed9f0b4bc1b..79da89bd9e981 100644 --- a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -21,8 +21,6 @@ #include #include -#include - namespace rviz_plugins { BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -69,16 +67,9 @@ void BagTimeManagerPanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); -// APIs taking rclcpp::QoS objects are only available in ROS 2 Iron and higher -#if RCLCPP_VERSION_MAJOR >= 18 - const auto qos_default = rclcpp::ServicesQoS(); -#else - const auto qos_default = rmw_qos_profile_services_default; -#endif - - client_pause_ = raw_node_->create_client("/rosbag2_player/pause", qos_default); - client_resume_ = raw_node_->create_client("/rosbag2_player/resume", qos_default); - client_set_rate_ = raw_node_->create_client("/rosbag2_player/set_rate", qos_default); + client_pause_ = raw_node_->create_client("/rosbag2_player/pause"); + client_resume_ = raw_node_->create_client("/rosbag2_player/resume"); + client_set_rate_ = raw_node_->create_client("/rosbag2_player/set_rate"); } void BagTimeManagerPanel::onPauseClicked() diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 5e795ae764092..1e61610797301 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -255,14 +255,14 @@ void RTCManagerPanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - client_rtc_commands_ = raw_node_->create_client( - "/api/external/set/rtc_commands", rmw_qos_profile_services_default); + client_rtc_commands_ = + raw_node_->create_client("/api/external/set/rtc_commands"); for (size_t i = 0; i < auto_modes_.size(); i++) { auto & a = auto_modes_.at(i); // auto mode - a->enable_auto_mode_cli = raw_node_->create_client( - enable_auto_mode_namespace_ + "/" + a->module_name, rmw_qos_profile_services_default); + a->enable_auto_mode_cli = + raw_node_->create_client(enable_auto_mode_namespace_ + "/" + a->module_name); } sub_rtc_status_ = raw_node_->create_subscription( diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp index 659ca64947952..cc8d46e2f60c6 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -61,22 +61,20 @@ void AutowareAutomaticGoalSender::initCommunication(rclcpp::Node * node) "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareAutomaticGoalSender::onOperationMode, this, std::placeholders::_1)); - cli_change_to_autonomous_ = node->create_client( - "/api/operation_mode/change_to_autonomous", rmw_qos_profile_services_default); + cli_change_to_autonomous_ = + node->create_client("/api/operation_mode/change_to_autonomous"); - cli_change_to_stop_ = node->create_client( - "/api/operation_mode/change_to_stop", rmw_qos_profile_services_default); + cli_change_to_stop_ = + node->create_client("/api/operation_mode/change_to_stop"); // Planning sub_route_ = node->create_subscription( "/api/routing/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareAutomaticGoalSender::onRoute, this, std::placeholders::_1)); - cli_clear_route_ = - node->create_client("/api/routing/clear_route", rmw_qos_profile_services_default); + cli_clear_route_ = node->create_client("/api/routing/clear_route"); - cli_set_route_ = node->create_client( - "/api/routing/set_route_points", rmw_qos_profile_services_default); + cli_set_route_ = node->create_client("/api/routing/set_route_points"); } // Sub diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index eebccd2fcd48f..9610bf1e2b7f1 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -176,8 +176,7 @@ void ManualController::onInitialize() sub_gear_ = raw_node_->create_subscription( "/vehicle/status/gear_status", 10, std::bind(&ManualController::onGear, this, _1)); - client_engage_ = raw_node_->create_client( - "/api/autoware/set/engage", rmw_qos_profile_services_default); + client_engage_ = raw_node_->create_client("/api/autoware/set/engage"); pub_gate_mode_ = raw_node_->create_publisher("/control/gate_mode_cmd", rclcpp::QoS(1)); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 3cc04347d2ed3..f9a5fbcbb2a07 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -219,31 +219,30 @@ void AutowareStatePanel::onInitialize() "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onOperationMode, this, _1)); - client_change_to_autonomous_ = raw_node_->create_client( - "/api/operation_mode/change_to_autonomous", rmw_qos_profile_services_default); + client_change_to_autonomous_ = + raw_node_->create_client("/api/operation_mode/change_to_autonomous"); - client_change_to_stop_ = raw_node_->create_client( - "/api/operation_mode/change_to_stop", rmw_qos_profile_services_default); + client_change_to_stop_ = + raw_node_->create_client("/api/operation_mode/change_to_stop"); - client_change_to_local_ = raw_node_->create_client( - "/api/operation_mode/change_to_local", rmw_qos_profile_services_default); + client_change_to_local_ = + raw_node_->create_client("/api/operation_mode/change_to_local"); - client_change_to_remote_ = raw_node_->create_client( - "/api/operation_mode/change_to_remote", rmw_qos_profile_services_default); + client_change_to_remote_ = + raw_node_->create_client("/api/operation_mode/change_to_remote"); - client_enable_autoware_control_ = raw_node_->create_client( - "/api/operation_mode/enable_autoware_control", rmw_qos_profile_services_default); + client_enable_autoware_control_ = + raw_node_->create_client("/api/operation_mode/enable_autoware_control"); - client_enable_direct_control_ = raw_node_->create_client( - "/api/operation_mode/disable_autoware_control", rmw_qos_profile_services_default); + client_enable_direct_control_ = + raw_node_->create_client("/api/operation_mode/disable_autoware_control"); // Routing sub_route_ = raw_node_->create_subscription( "/api/routing/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onRoute, this, _1)); - client_clear_route_ = raw_node_->create_client( - "/api/routing/clear_route", rmw_qos_profile_services_default); + client_clear_route_ = raw_node_->create_client("/api/routing/clear_route"); // Localization sub_localization_ = raw_node_->create_subscription( @@ -255,8 +254,7 @@ void AutowareStatePanel::onInitialize() "/api/motion/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMotion, this, _1)); - client_accept_start_ = raw_node_->create_client( - "/api/motion/accept_start", rmw_qos_profile_services_default); + client_accept_start_ = raw_node_->create_client("/api/motion/accept_start"); // FailSafe sub_mrm_ = raw_node_->create_subscription( @@ -271,7 +269,7 @@ void AutowareStatePanel::onInitialize() "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); client_emergency_stop_ = raw_node_->create_client( - "/api/autoware/set/emergency", rmw_qos_profile_services_default); + "/api/autoware/set/emergency"); pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 53d901ac4c780..897986a7a41cf 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -511,8 +511,8 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Waiting for emergency_stop service connection..."); } - client_autoware_engage_ = this->create_client( - "service/autoware_engage", rmw_qos_profile_services_default); + client_autoware_engage_ = + this->create_client("service/autoware_engage"); // Timer initTimer(1.0 / update_rate_); diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 8ea0d38ff1e1b..bcd93e78c1aad 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -67,8 +67,8 @@ MapUpdateModule::MapUpdateModule( &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_); - pcd_loader_client_ = node->create_client( - "pcd_loader_service", rmw_qos_profile_services_default); + pcd_loader_client_ = + node->create_client("pcd_loader_service"); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { RCLCPP_INFO( logger_, diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index 33970700991c7..d7ccfdff777f9 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -71,8 +71,8 @@ RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( using std::placeholders::_2; // Service client - enable_cli_ = node->create_client( - enable_auto_mode_namespace_ + "/internal/" + module_name, rmw_qos_profile_services_default); + enable_cli_ = + node->create_client(enable_auto_mode_namespace_ + "/internal/" + module_name); while (!enable_cli_->wait_for_service(1s)) { if (!rclcpp::ok()) { diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index ebc7171fa209b..333beafc12399 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -92,8 +92,7 @@ RTCReplayerNode::RTCReplayerNode(const rclcpp::NodeOptions & node_options) { sub_statuses_ = create_subscription( "/debug/rtc_status", 1, std::bind(&RTCReplayerNode::onCooperateStatus, this, _1)); - client_rtc_commands_ = create_client( - "/api/external/set/rtc_commands", rmw_qos_profile_services_default); + client_rtc_commands_ = create_client("/api/external/set/rtc_commands"); } void RTCReplayerNode::onCooperateStatus(const CooperateStatusArray::ConstSharedPtr msg) From 4bc0ece582ec3ff077234158bdfd0c0410ffdd00 Mon Sep 17 00:00:00 2001 From: ito-san <57388357+ito-san@users.noreply.github.com> Date: Thu, 25 May 2023 17:06:55 +0900 Subject: [PATCH 4/9] feat(system_monitor): add detection of ECC memory errors (#3795) * feat(system_monitor): add detection of ECC memory errors Signed-off-by: ito-san * style(pre-commit): autofix * fix process crash when edac-utils is not installed Signed-off-by: ito-san * style(pre-commit): autofix --------- Signed-off-by: ito-san Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../mem_monitor/mem_monitor.hpp | 6 ++ .../src/mem_monitor/mem_monitor.cpp | 56 +++++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/system/system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp b/system/system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp index 11bf70e7fdc73..452e7bd29171b 100644 --- a/system/system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp +++ b/system/system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp @@ -52,6 +52,12 @@ class MemMonitor : public rclcpp::Node void checkUsage( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief check Memory ECC + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + */ + void checkEcc(diagnostic_updater::DiagnosticStatusWrapper & stat); + /** * @brief get human-readable output for memory size * @param [in] str size with bytes diff --git a/system/system_monitor/src/mem_monitor/mem_monitor.cpp b/system/system_monitor/src/mem_monitor/mem_monitor.cpp index 7a76f84612da8..4d141294c3fa5 100644 --- a/system/system_monitor/src/mem_monitor/mem_monitor.cpp +++ b/system/system_monitor/src/mem_monitor/mem_monitor.cpp @@ -38,6 +38,11 @@ MemMonitor::MemMonitor(const rclcpp::NodeOptions & options) gethostname(hostname_, sizeof(hostname_)); updater_.setHardwareID(hostname_); updater_.add("Memory Usage", this, &MemMonitor::checkUsage); + + // Enable ECC error detection if edac-utils package is installed + if (!bp::search_path("edac-util").empty()) { + updater_.add("Memory ECC", this, &MemMonitor::checkEcc); + } } void MemMonitor::update() @@ -156,6 +161,57 @@ void MemMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) SystemMonitorUtility::stopMeasurement(t_start, stat); } +void MemMonitor::checkEcc(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + + bp::child c("edac-util --quiet", bp::std_out > is_out, bp::std_err > is_err); + c.wait(); + if (c.exit_code() != 0) { + std::ostringstream os; + is_err >> os.rdbuf(); + stat.summary(DiagStatus::ERROR, "edac-util error"); + stat.add("edac-util", os.str().c_str()); + return; + } + + std::string line; + + /* + Output example of `edac-util --quiet` + edac-util generates output if error occurred, otherwise no output + mc0: 3 Uncorrected Errors with no DIMM info + mc0: 3 Corrected Errors with no DIMM info + */ + while (std::getline(is_out, line)) { + if (line.find("Uncorrected") != std::string::npos) { + stat.summary(DiagStatus::ERROR, line); + return; + } else if (line.find("Corrected") != std::string::npos) { + stat.summary(DiagStatus::WARN, line); + return; + } + } + + stat.summary(DiagStatus::OK, "OK"); +} + std::string MemMonitor::toHumanReadable(const std::string & str) { const char * units[] = {"B", "K", "M", "G", "T"}; From 938503d8e437084ed95a3144b96bbcd9e0ecce12 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 25 May 2023 17:18:33 +0900 Subject: [PATCH 5/9] fix(autonomous_emergency_braking): fix imu tranform to base_link (#3812) --- .../autonomous_emergency_braking/node.hpp | 4 +++- .../autonomous_emergency_braking/src/node.cpp | 20 ++++++++++++++++--- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index ae38aa30a1938..8859367c25e9b 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,7 @@ using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; struct ObjectData { @@ -155,7 +157,7 @@ class AEB : public rclcpp::Node PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; - Imu::ConstSharedPtr imu_ptr_{nullptr}; + Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 03ae761733f77..7e10ca9864f11 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -154,7 +154,21 @@ void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) void AEB::onImu(const Imu::ConstSharedPtr input_msg) { - imu_ptr_ = input_msg; + // transform imu + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + angular_velocity_ptr_ = std::make_shared(); + tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); } void AEB::onPredictedTrajectory( @@ -225,7 +239,7 @@ bool AEB::isDataReady() return missing("object pointcloud"); } - if (use_imu_path_ && !imu_ptr_) { + if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } @@ -286,7 +300,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) if (use_imu_path_) { Path ego_path; std::vector ego_polys; - const double current_w = imu_ptr_->angular_velocity.z; + const double current_w = angular_velocity_ptr_->z; constexpr double color_r = 0.0 / 256.0; constexpr double color_g = 148.0 / 256.0; constexpr double color_b = 205.0 / 256.0; From b7f3832781e34c983b24abbb9d7a9a489298ae2b Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 25 May 2023 18:55:50 +0900 Subject: [PATCH 6/9] refactor(mission_planner): clean reroute code (#3815) Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 139 ++++++++---------- .../src/mission_planner/mission_planner.hpp | 2 + 2 files changed, 62 insertions(+), 79 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index acb25028b8acf..8d4936258edb8 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -169,6 +169,59 @@ void MissionPlanner::change_route(const LaneletRoute & route) normal_route_ = std::make_shared(route); } +LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) +{ + PoseStamped goal_pose; + goal_pose.header = req->header; + goal_pose.pose = req->goal; + + // Convert route. + LaneletRoute route; + route.start_pose = odometry_->pose.pose; + route.goal_pose = transform_pose(goal_pose).pose; + for (const auto & segment : req->segments) { + route.segments.push_back(convert(segment)); + } + route.header.stamp = req->header.stamp; + route.header.frame_id = map_frame_; + route.uuid.uuid = generate_random_id(); + route.allow_modification = req->option.allow_goal_modification; + + return route; +} + +LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req) +{ + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + + // Use temporary pose stamped for transform. + PoseStamped pose; + pose.header = req->header; + + // Convert route points. + PlannerPlugin::RoutePoints points; + points.push_back(odometry_->pose.pose); + for (const auto & waypoint : req->waypoints) { + pose.pose = waypoint; + points.push_back(transform_pose(pose).pose); + } + pose.pose = req->goal; + points.push_back(transform_pose(pose).pose); + + // Plan route. + LaneletRoute route = planner_->plan(points); + if (route.segments.empty()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + route.header.stamp = req->header.stamp; + route.header.frame_id = map_frame_; + route.uuid.uuid = generate_random_id(); + route.allow_modification = req->option.allow_goal_modification; + + return route; +} + void MissionPlanner::change_state(RouteState::Message::_state_type state) { state_.stamp = now(); @@ -200,22 +253,8 @@ void MissionPlanner::on_set_route( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - // Use temporary pose stamped for transform. - PoseStamped pose; - pose.header = req->header; - pose.pose = req->goal; - - // Convert route. - LaneletRoute route; - route.start_pose = odometry_->pose.pose; - route.goal_pose = transform_pose(pose).pose; - for (const auto & segment : req->segments) { - route.segments.push_back(convert(segment)); - } - route.header.stamp = req->header.stamp; - route.header.frame_id = map_frame_; - route.uuid.uuid = generate_random_id(); - route.allow_modification = req->option.allow_goal_modification; + // Convert request to a new route. + const auto route = create_route(req); // Update route. change_route(route); @@ -244,30 +283,8 @@ void MissionPlanner::on_set_route_points( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - // Use temporary pose stamped for transform. - PoseStamped pose; - pose.header = req->header; - - // Convert route points. - PlannerPlugin::RoutePoints points; - points.push_back(odometry_->pose.pose); - for (const auto & waypoint : req->waypoints) { - pose.pose = waypoint; - points.push_back(transform_pose(pose).pose); - } - pose.pose = req->goal; - points.push_back(transform_pose(pose).pose); - // Plan route. - LaneletRoute route = planner_->plan(points); - if (route.segments.empty()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); - } - route.header.stamp = req->header.stamp; - route.header.frame_id = map_frame_; - route.uuid.uuid = generate_random_id(); - route.allow_modification = req->option.allow_goal_modification; + const auto route = create_route(req); // Update route. change_route(route); @@ -316,24 +333,11 @@ void MissionPlanner::on_change_route( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + // set to changing state change_state(RouteState::Message::CHANGING); - // Use temporary pose stamped for transform. - PoseStamped pose; - pose.header = req->header; - pose.pose = req->goal; - - // Convert route. - LaneletRoute new_route; - new_route.start_pose = odometry_->pose.pose; - new_route.goal_pose = transform_pose(pose).pose; - for (const auto & segment : req->segments) { - new_route.segments.push_back(convert(segment)); - } - new_route.header.stamp = req->header.stamp; - new_route.header.frame_id = map_frame_; - new_route.uuid.uuid = generate_random_id(); - new_route.allow_modification = req->option.allow_goal_modification; + // Convert request to a new route. + const auto new_route = create_route(req); // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { @@ -373,31 +377,8 @@ void MissionPlanner::on_change_route_points( change_state(RouteState::Message::CHANGING); - // Use temporary pose stamped for transform. - PoseStamped pose; - pose.header = req->header; - - // Convert route points. - PlannerPlugin::RoutePoints points; - points.push_back(odometry_->pose.pose); - for (const auto & waypoint : req->waypoints) { - pose.pose = waypoint; - points.push_back(transform_pose(pose).pose); - } - pose.pose = req->goal; - points.push_back(transform_pose(pose).pose); - // Plan route. - LaneletRoute new_route = planner_->plan(points); - if (new_route.segments.empty()) { - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); - } - new_route.header.stamp = req->header.stamp; - new_route.header.frame_id = map_frame_; - new_route.uuid.uuid = generate_random_id(); - new_route.allow_modification = req->option.allow_goal_modification; + const auto new_route = create_route(req); // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 1c554ca339958..0612114f8ce1a 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -76,6 +76,8 @@ class MissionPlanner : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); void change_route(const LaneletRoute & route); + LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req); + LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req); RouteState::Message state_; component_interface_utils::Publisher::SharedPtr pub_state_; From 7aaaef2dc650f0aac3c9e322f4a65c9d421f2625 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 25 May 2023 19:05:39 +0900 Subject: [PATCH 7/9] fix(concatedate_date): add error handling when sensor points is empty (#3814) fix(concatedate_date): add error handling when sensor points are empty --- .../src/concatenate_data/concatenate_data_nodelet.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index fbcce3b304d7e..d16323b7fc602 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -327,6 +327,13 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; + + if (input_ptr->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); From 41f5bc1e4d2f62fea50508a51eb7299bebfe6f2f Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 25 May 2023 20:14:33 +0900 Subject: [PATCH 8/9] feat(mission_planner): add minimum checking length (#3816) * refactor(mission_planner): clean reroute code Signed-off-by: yutaka * feat(mission_planner): add minimum reroute length Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- planning/mission_planner/README.md | 1 + planning/mission_planner/config/mission_planner.param.yaml | 1 + .../mission_planner/src/mission_planner/mission_planner.cpp | 5 ++++- .../mission_planner/src/mission_planner/mission_planner.hpp | 1 + 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 3f9b54812f29f..fb062f4d663f6 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -24,6 +24,7 @@ In current Autoware.universe, only Lanelet2 map format is supported. | `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | | `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | | `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | +| `minimum_reroute_length` | double | Minimum Length for publishing a new route | ### Services diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index fe84be3b2f421..d596eb9816f0a 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -7,3 +7,4 @@ goal_angle_threshold_deg: 45.0 enable_correct_goal_pose: false reroute_time_threshold: 10.0 + minimum_reroute_length: 30.0 diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 8d4936258edb8..fd1dabbc58dc1 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -80,6 +80,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) { map_frame_ = declare_parameter("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); + minimum_reroute_length_ = declare_parameter("minimum_reroute_length"); planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); @@ -492,7 +493,9 @@ bool MissionPlanner::checkRerouteSafety( // check safety const auto current_velocity = odometry_->twist.twist.linear.x; - if (accumulated_length > current_velocity * reroute_time_threshold_) { + const double safety_length = + std::max(current_velocity * reroute_time_threshold_, minimum_reroute_length_); + if (accumulated_length > safety_length) { return true; } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 0612114f8ce1a..f108e2d99496c 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -121,6 +121,7 @@ class MissionPlanner : public rclcpp::Node const SetRoutePoints::Service::Response::SharedPtr res); double reroute_time_threshold_{10.0}; + double minimum_reroute_length_{30.0}; bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::shared_ptr original_route_{nullptr}; From 712c11c5cb70d0a8b7e298e4777c4d5e3fb2bbea Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Thu, 25 May 2023 20:14:43 +0900 Subject: [PATCH 9/9] fix(mission_planner): subtract goal distance from the accumulated length (#3818) * refactor(mission_planner): clean reroute code Signed-off-by: yutaka * feat(mission_planner): add minimum reroute length Signed-off-by: yutaka * fix(mission_planner): subtract goal distance from safety distance Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../src/mission_planner/mission_planner.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index fd1dabbc58dc1..b3df2d0a0010a 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -491,6 +491,25 @@ bool MissionPlanner::checkRerouteSafety( accumulated_length += *std::min_element(lanelets_length.begin(), lanelets_length.end()); } + // check if the goal is inside of the target terminal lanelet + const auto & target_end_primitives = target_route.segments.at(end_idx - start_idx).primitives; + const auto & target_goal = target_route.goal_pose; + for (const auto & target_end_primitive : target_end_primitives) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id); + if (lanelet::utils::isInLanelet(target_goal, lanelet)) { + const auto target_goal_position = + lanelet::utils::conversion::toLaneletPoint(target_goal.position); + const double dist_to_goal = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(lanelet.centerline()), + lanelet::utils::to2D(target_goal_position).basicPoint()) + .length; + const double target_lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + const double dist = target_lanelet_length - dist_to_goal; + accumulated_length = std::max(accumulated_length - dist, 0.0); + break; + } + } + // check safety const auto current_velocity = odometry_->twist.twist.linear.x; const double safety_length =