From 1a095226af098dfc03f5840f51cc0695a7c3be42 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 12 Oct 2023 09:58:39 +0900 Subject: [PATCH 01/48] wip Signed-off-by: Tomohito Ando --- system/mrm_pull_over_manager/CMakeLists.txt | 15 +++ system/mrm_pull_over_manager/README.md | 28 ++++ .../config/mrm_pull_over_manager.param.yaml | 14 ++ .../mrm_pull_over_manager_core.hpp | 89 +++++++++++++ .../launch/mrm_pull_over_manager.launch.xml | 35 +++++ system/mrm_pull_over_manager/package.xml | 32 +++++ .../mrm_pull_over_manager_core.cpp | 121 ++++++++++++++++++ .../mrm_pull_over_manager_node.cpp | 32 +++++ 8 files changed, 366 insertions(+) create mode 100644 system/mrm_pull_over_manager/CMakeLists.txt create mode 100644 system/mrm_pull_over_manager/README.md create mode 100644 system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml create mode 100644 system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp create mode 100644 system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml create mode 100644 system/mrm_pull_over_manager/package.xml create mode 100644 system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp create mode 100644 system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp diff --git a/system/mrm_pull_over_manager/CMakeLists.txt b/system/mrm_pull_over_manager/CMakeLists.txt new file mode 100644 index 0000000000000..10e36f9746f71 --- /dev/null +++ b/system/mrm_pull_over_manager/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_pull_over_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(mrm_pull_over_manager + src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp + src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_pull_over_manager/README.md b/system/mrm_pull_over_manager/README.md new file mode 100644 index 0000000000000..efc659e162e79 --- /dev/null +++ b/system/mrm_pull_over_manager/README.md @@ -0,0 +1,28 @@ +# mrm_pull_over_manager + +## Purpose + + +## Inner-workings / Algorithms + +### State Transitions + +## Inputs / Outputs + +### Input + + +### Output + + +## Parameters + +### Node Parameters + + +### Core Parameters + + +## Assumptions / Known limits + +TBD. diff --git a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml new file mode 100644 index 0000000000000..652a984ce539a --- /dev/null +++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml @@ -0,0 +1,14 @@ +# Default configuration for emergency handler +--- +/**: + ros__parameters: + update_rate: 10 + timeout_hazard_status: 0.5 + timeout_takeover_request: 10.0 + use_takeover_request: false + use_parking_after_stopped: false + use_comfortable_stop: false + + # setting whether to turn hazard lamp on for each situation + turning_hazard_on: + emergency: true diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp new file mode 100644 index 0000000000000..f356b72688d37 --- /dev/null +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -0,0 +1,89 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ +#define MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ + +// Core +#include +#include + +// Autoware +#include + +#include + +// lanelet +#include +#include + +#include +#include +#include +#include +#include +#include + +// ROS 2 core +#include + +#include + +class MrmPullOverManager : public rclcpp::Node +{ +public: + MrmPullOverManager(); + +private: + using Odometry = nav_msgs::msg::Odometry; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + + // Subscribers + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_map_; + + Odometry::ConstSharedPtr odom_; + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; + lanelet::ConstLanelets road_lanelets_; + lanelet::ConstLanelets shoulder_lanelets_; + route_handler::RouteHandler route_handler_; + + void onOdometry(const Odometry::ConstSharedPtr msg); + void onMap(const HADMapBin::ConstSharedPtr msg); + + // Publisher + // rclcpp::Publisher::SharedPtr + // pub_control_command_; + + // void publishMrmState(); + + // Clients + // rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; + // rclcpp::Client::SharedPtr client_mrm_comfortable_stop_; + + // Timer + // rclcpp::TimerBase::SharedPtr timer_; + + // Parameters + // Param param_; + + bool isDataReady(); + + // Algorithm +}; + +#endif // MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml new file mode 100644 index 0000000000000..445757815c2cc --- /dev/null +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/mrm_pull_over_manager/package.xml b/system/mrm_pull_over_manager/package.xml new file mode 100644 index 0000000000000..7f7f2db7bd35f --- /dev/null +++ b/system/mrm_pull_over_manager/package.xml @@ -0,0 +1,32 @@ + + + + mrm_pull_over_manager + 0.1.0 + The mrm_pull_over_manager ROS 2 package + Makoto Kurihara + Tomohito Ando + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + nav_msgs + rclcpp + std_msgs + std_srvs + tier4_system_msgs + lanelet2_extension + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp new file mode 100644 index 0000000000000..acd4d4959c0b7 --- /dev/null +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mrm_pull_over_manager/mrm_pull_over_manager_core.hpp" + +#include +#include +#include + +MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") +{ + // Parameter + // param_.update_rate = declare_parameter("update_rate", 10); + // param_.timeout_hazard_status = declare_parameter("timeout_hazard_status", 0.5); + // param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); + // param_.use_takeover_request = declare_parameter("use_takeover_request", false); + // param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); + // param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); + // param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", + // true); + + using std::placeholders::_1; + + // Subscriber + sub_odom_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmPullOverManager::onOdometry, this, _1)); + sub_map_ = create_subscription( + "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MrmPullOverManager::onMap, this, _1)); + + // Publisher + // pub_control_command_ = + // create_publisher( + // "~/output/control_command", rclcpp::QoS{1}); + // pub_hazard_cmd_ = create_publisher( + // "~/output/hazard", rclcpp::QoS{1}); + // pub_gear_cmd_ = + // create_publisher("~/output/gear", + // rclcpp::QoS{1}); + // pub_mrm_state_ = + // create_publisher("~/output/mrm/state", + // rclcpp::QoS{1}); + + // Clients + // client_mrm_comfortable_stop_group_ = + // create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // client_mrm_comfortable_stop_ = create_client( + // "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, + // client_mrm_comfortable_stop_group_); +} + +void MrmPullOverManager::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + odom_ = msg; +} + +void MrmPullOverManager::onMap(const HADMapBin::ConstSharedPtr msg) +{ + // lanelet_map_ptr_ = std::make_shared(); + // lanelet::utils::conversion::fromBinMsg( + // *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + // const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + // lanelet::Locations::Germany, lanelet::Participants::Vehicle); + // const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + // lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + // lanelet::routing::RoutingGraphConstPtr vehicle_graph = + // lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + // lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + // lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + // lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + // overall_graphs_ptr_ = + // std::make_shared(overall_graphs); + + route_handler_.setMap(*msg); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); +} + +bool MrmPullOverManager::isDataReady() +{ + // if (!hazard_status_stamped_) { + // RCLCPP_INFO_THROTTLE( + // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + // "waiting for hazard_status_stamped msg..."); + // return false; + // } + + // if ( + // param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == + // tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + // RCLCPP_INFO_THROTTLE( + // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + // "waiting for mrm comfortable stop to become available..."); + // return false; + // } + + // if ( + // mrm_emergency_stop_status_->state == + // tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { RCLCPP_INFO_THROTTLE( + // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + // "waiting for mrm emergency stop to become available..."); + // return false; + // } + + return true; +} diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp new file mode 100644 index 0000000000000..64a89d99cdcbc --- /dev/null +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp @@ -0,0 +1,32 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mrm_pull_over_manager/mrm_pull_over_manager_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + + return 0; +} From 0fa11f4d157bc9df4667d0381024d730dcf785d3 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 12 Oct 2023 18:02:52 +0900 Subject: [PATCH 02/48] wip Signed-off-by: Tomohito Ando --- .../launch/system.launch.xml | 4 + .../config/mrm_pull_over_manager.param.yaml | 11 +- .../mrm_pull_over_manager_core.hpp | 44 ++-- .../launch/mrm_pull_over_manager.launch.xml | 27 +-- system/mrm_pull_over_manager/package.xml | 1 + .../mrm_pull_over_manager_core.cpp | 201 +++++++++++++----- .../mrm_pull_over_manager_node.cpp | 2 +- 7 files changed, 189 insertions(+), 101 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 016d613cfa72d..e1f1833b54470 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -95,6 +95,10 @@ + + + + diff --git a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml index 652a984ce539a..799fdc39ad5b8 100644 --- a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml +++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml @@ -2,13 +2,4 @@ --- /**: ros__parameters: - update_rate: 10 - timeout_hazard_status: 0.5 - timeout_takeover_request: 10.0 - use_takeover_request: false - use_parking_after_stopped: false - use_comfortable_stop: false - - # setting whether to turn hazard lamp on for each situation - turning_hazard_on: - emergency: true + test: 10 diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index f356b72688d37..bfb07c6d96278 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -17,7 +17,7 @@ // Core #include -#include +#include // Autoware #include @@ -27,6 +27,7 @@ // lanelet #include #include +#include #include #include @@ -39,7 +40,14 @@ #include #include +namespace mrm_pull_over_manager +{ +struct PoseWithLaneId +{ + geometry_msgs::msg::Pose pose; + lanelet::Id lane_id; +}; class MrmPullOverManager : public rclcpp::Node { public: @@ -48,42 +56,44 @@ class MrmPullOverManager : public rclcpp::Node private: using Odometry = nav_msgs::msg::Odometry; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_map_; Odometry::ConstSharedPtr odom_; - lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; - lanelet::routing::RoutingGraphPtr routing_graph_ptr_; - std::shared_ptr overall_graphs_ptr_; - lanelet::ConstLanelets road_lanelets_; - lanelet::ConstLanelets shoulder_lanelets_; route_handler::RouteHandler route_handler_; - void onOdometry(const Odometry::ConstSharedPtr msg); - void onMap(const HADMapBin::ConstSharedPtr msg); + void on_odometry(const Odometry::ConstSharedPtr msg); + void on_route(const LaneletRoute::ConstSharedPtr msg); + void on_map(const HADMapBin::ConstSharedPtr msg); // Publisher - // rclcpp::Publisher::SharedPtr - // pub_control_command_; - - // void publishMrmState(); // Clients - // rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; - // rclcpp::Client::SharedPtr client_mrm_comfortable_stop_; + // TODO: temporary for debug // Timer - // rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer_; + void on_timer(); // Parameters // Param param_; - bool isDataReady(); + std::vector candidate_goals_; // Algorithm + bool is_data_ready(); + std::vector find_near_goals(); + lanelet::ConstLanelet get_current_lanelet(); + lanelet::ConstLanelets get_all_following_and_left_lanelets( + const lanelet::ConstLanelet & start_lanelet) const; + lanelet::ConstLanelets get_all_left_lanelets(const lanelet::ConstLanelet & base_lanelet) const; + std::vector find_goals_in_lanelets( + const lanelet::ConstLanelets & candidate_lanelets) const; }; +} // namespace mrm_pull_over_manager #endif // MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml index 445757815c2cc..5d2854f178ab3 100644 --- a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -1,34 +1,19 @@ - - - - - - + + - - - - - + - - - - - + + - - - - - + diff --git a/system/mrm_pull_over_manager/package.xml b/system/mrm_pull_over_manager/package.xml index 7f7f2db7bd35f..e58b01f1db923 100644 --- a/system/mrm_pull_over_manager/package.xml +++ b/system/mrm_pull_over_manager/package.xml @@ -22,6 +22,7 @@ std_srvs tier4_system_msgs lanelet2_extension + route_handler ament_lint_auto autoware_lint_common diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index acd4d4959c0b7..dc93a7f935e30 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -14,9 +14,24 @@ #include "mrm_pull_over_manager/mrm_pull_over_manager_core.hpp" -#include -#include -#include +namespace mrm_pull_over_manager +{ + +namespace +{ +PoseWithLaneId create_pose_with_lane_id( + const double x, const double y, const double z, const lanelet::Id lanelet_id) +{ + PoseWithLaneId pose_with_lane_id; + pose_with_lane_id.pose = geometry_msgs::msg::Pose(); + pose_with_lane_id.pose.position.x = x; + pose_with_lane_id.pose.position.y = y; + pose_with_lane_id.pose.position.z = z; + pose_with_lane_id.lane_id = lanelet_id; + + return pose_with_lane_id; +} +} // namespace MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") { @@ -34,10 +49,13 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") // Subscriber sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmPullOverManager::onOdometry, this, _1)); + "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmPullOverManager::on_odometry, this, _1)); + sub_route_ = this->create_subscription( + "~/input/route", rclcpp::QoS{1}.transient_local(), + std::bind(&MrmPullOverManager::on_route, this, _1)); sub_map_ = create_subscription( - "~/input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MrmPullOverManager::onMap, this, _1)); + "~/input/lanelet_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MrmPullOverManager::on_map, this, _1)); // Publisher // pub_control_command_ = @@ -58,64 +76,143 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") // client_mrm_comfortable_stop_ = create_client( // "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, // client_mrm_comfortable_stop_group_); + + // TODO: temporary + // Timer + const auto update_period_ns = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&MrmPullOverManager::on_timer, this)); } -void MrmPullOverManager::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void MrmPullOverManager::on_timer() +{ + if (is_data_ready()) { + const auto emergency_goals = find_near_goals(); + } +} +void MrmPullOverManager::on_odometry(const Odometry::ConstSharedPtr msg) { odom_ = msg; } -void MrmPullOverManager::onMap(const HADMapBin::ConstSharedPtr msg) +void MrmPullOverManager::on_route(const LaneletRoute::ConstSharedPtr msg) { - // lanelet_map_ptr_ = std::make_shared(); - // lanelet::utils::conversion::fromBinMsg( - // *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - // const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - // lanelet::Locations::Germany, lanelet::Participants::Vehicle); - // const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - // lanelet::Locations::Germany, lanelet::Participants::Pedestrian); - // lanelet::routing::RoutingGraphConstPtr vehicle_graph = - // lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); - // lanelet::routing::RoutingGraphConstPtr pedestrian_graph = - // lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); - // lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); - // overall_graphs_ptr_ = - // std::make_shared(overall_graphs); + route_handler_.setRoute(*msg); +} +void MrmPullOverManager::on_map(const HADMapBin::ConstSharedPtr msg) +{ route_handler_.setMap(*msg); - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } -bool MrmPullOverManager::isDataReady() +bool MrmPullOverManager::is_data_ready() { - // if (!hazard_status_stamped_) { - // RCLCPP_INFO_THROTTLE( - // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - // "waiting for hazard_status_stamped msg..."); - // return false; - // } - - // if ( - // param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - // tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { - // RCLCPP_INFO_THROTTLE( - // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - // "waiting for mrm comfortable stop to become available..."); - // return false; - // } - - // if ( - // mrm_emergency_stop_status_->state == - // tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { RCLCPP_INFO_THROTTLE( - // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), - // "waiting for mrm emergency stop to become available..."); - // return false; - // } + if (!odom_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for odometry msg..."); + return false; + } + + if (!route_handler_.isHandlerReady()) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Waiting for route handler."); + return false; + } return true; } + +std::vector MrmPullOverManager::find_near_goals() +{ + const auto current_lanelet = get_current_lanelet(); + RCLCPP_INFO(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); + + const auto candidate_lanelets = get_all_following_and_left_lanelets(current_lanelet); + + candidate_goals_.emplace_back(create_pose_with_lane_id(1, 1, 1, 19464)); + candidate_goals_.emplace_back(create_pose_with_lane_id(2, 2, 2, 19779)); + candidate_goals_.emplace_back(create_pose_with_lane_id(3, 3, 3, 20976)); + candidate_goals_.emplace_back(create_pose_with_lane_id(4, 4, 4, 20478)); + const auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); + + return emergency_goals; +} + +/** + * @brief get current root lanelet + * @param + * @return current lanelet + */ +lanelet::ConstLanelet MrmPullOverManager::get_current_lanelet() +{ + lanelet::ConstLanelet current_lanelet; + route_handler_.getClosestLaneletWithinRoute(odom_->pose.pose, ¤t_lanelet); + + return current_lanelet; +} + +/** + * @brief get all lanes followed by start_lanelet and left adjacent lanes. + * @param start_lanelet + * @return current lanelet + */ +lanelet::ConstLanelets MrmPullOverManager::get_all_following_and_left_lanelets( + const lanelet::ConstLanelet & start_lanelet) const +{ + lanelet::ConstLanelet base_lanelet = start_lanelet; + lanelet::ConstLanelet next_lanelet; + lanelet::ConstLanelets result_lanelets; + while (true) { + RCLCPP_INFO(this->get_logger(), "base lanelet id: %ld", base_lanelet.id()); + result_lanelets.emplace_back(base_lanelet); + + // If there are multiple lanes to the left of the current lane, retrieve all of them + const auto left_lanelets = get_all_left_lanelets(base_lanelet); + result_lanelets.insert(result_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + + // search next following lanelet + const bool has_next = route_handler_.getNextLaneletWithinRoute(base_lanelet, &next_lanelet); + if (!has_next) { + break; + } + base_lanelet = next_lanelet; + } + + return result_lanelets; +} + +/** + * @brief get all lanes followed by state_lanelet and left adjacent lanes of those lanes. + * @param base_lanelet + * @return + */ +lanelet::ConstLanelets MrmPullOverManager::get_all_left_lanelets( + const lanelet::ConstLanelet & base_lanelet) const +{ + lanelet::ConstLanelets left_lanalets; + auto left_lanelet = route_handler_.getLeftLanelet(base_lanelet, false, true); + while (left_lanelet) { + left_lanalets.emplace_back(left_lanelet.get()); + RCLCPP_INFO(this->get_logger(), "left lanelet id: %ld", left_lanelet->id()); + + // get next left lanelet + left_lanelet = route_handler_.getLeftLanelet(left_lanelet.get(), false, true); + } + + return left_lanalets; +} + +/** + * @brief + * @param candidate_lanelets + * @return + */ +std::vector MrmPullOverManager::find_goals_in_lanelets( + const lanelet::ConstLanelets & candidate_lanelets) const +{ + std::vector goals; + return goals; +} + +} // namespace mrm_pull_over_manager diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp index 64a89d99cdcbc..8aa5ac1462690 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp @@ -22,7 +22,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); + auto node = std::make_shared(); executor.add_node(node); executor.spin(); executor.remove_node(node); From 9b7857f3165cf72726acd901dcfe2fce9811bd7b Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 11:16:29 +0900 Subject: [PATCH 03/48] wip Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.hpp | 10 +-- .../mrm_pull_over_manager_core.cpp | 80 ++++++++++++------- 2 files changed, 52 insertions(+), 38 deletions(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index bfb07c6d96278..f1ca6109bc51b 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -16,6 +16,7 @@ #define MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ // Core +#include #include #include @@ -42,12 +43,6 @@ #include namespace mrm_pull_over_manager { - -struct PoseWithLaneId -{ - geometry_msgs::msg::Pose pose; - lanelet::Id lane_id; -}; class MrmPullOverManager : public rclcpp::Node { public: @@ -57,6 +52,7 @@ class MrmPullOverManager : public rclcpp::Node using Odometry = nav_msgs::msg::Odometry; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; + using PoseLaneIdMap = std::map; // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; @@ -82,7 +78,7 @@ class MrmPullOverManager : public rclcpp::Node // Parameters // Param param_; - std::vector candidate_goals_; + PoseLaneIdMap candidate_goals_; // Algorithm bool is_data_ready(); diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index dc93a7f935e30..8fa44a1f5a067 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -17,22 +17,24 @@ namespace mrm_pull_over_manager { +// TODO: あとでけす namespace { -PoseWithLaneId create_pose_with_lane_id( - const double x, const double y, const double z, const lanelet::Id lanelet_id) +geometry_msgs::msg::Pose create_pose(const double x, const double y, const double z) { - PoseWithLaneId pose_with_lane_id; - pose_with_lane_id.pose = geometry_msgs::msg::Pose(); - pose_with_lane_id.pose.position.x = x; - pose_with_lane_id.pose.position.y = y; - pose_with_lane_id.pose.position.z = z; - pose_with_lane_id.lane_id = lanelet_id; - - return pose_with_lane_id; + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + + return pose; } } // namespace +namespace lanelet_util +{ +} // namespace lanelet_util + MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") { // Parameter @@ -130,12 +132,21 @@ std::vector MrmPullOverManager::find_near_goals() const auto candidate_lanelets = get_all_following_and_left_lanelets(current_lanelet); - candidate_goals_.emplace_back(create_pose_with_lane_id(1, 1, 1, 19464)); - candidate_goals_.emplace_back(create_pose_with_lane_id(2, 2, 2, 19779)); - candidate_goals_.emplace_back(create_pose_with_lane_id(3, 3, 3, 20976)); - candidate_goals_.emplace_back(create_pose_with_lane_id(4, 4, 4, 20478)); + // candidate_goals_.emplace_back(create_pose_with_lane_id(1, 1, 1, 19464)); + // candidate_goals_.emplace_back(create_pose_with_lane_id(2, 2, 2, 19779)); + // candidate_goals_.emplace_back(create_pose_with_lane_id(3, 3, 3, 20976)); + // candidate_goals_.emplace_back(create_pose_with_lane_id(4, 4, 4, 20478)); + candidate_goals_[19464] = create_pose(1, 1, 1); + candidate_goals_[19779] = create_pose(2, 2, 2); + candidate_goals_[20976] = create_pose(3, 3, 3); + candidate_goals_[20478] = create_pose(4, 4, 4); const auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); + RCLCPP_INFO(this->get_logger(), "---found goals---"); + for (const auto & goal : emergency_goals) { + RCLCPP_INFO(this->get_logger(), "goal x: %f", goal.position.x); + } + return emergency_goals; } @@ -153,37 +164,33 @@ lanelet::ConstLanelet MrmPullOverManager::get_current_lanelet() } /** - * @brief get all lanes followed by start_lanelet and left adjacent lanes. + * @brief get all following lanes and left adjacent lanes from start_lanelet. * @param start_lanelet * @return current lanelet */ lanelet::ConstLanelets MrmPullOverManager::get_all_following_and_left_lanelets( const lanelet::ConstLanelet & start_lanelet) const { - lanelet::ConstLanelet base_lanelet = start_lanelet; - lanelet::ConstLanelet next_lanelet; + lanelet::ConstLanelet current_lanelet = start_lanelet; lanelet::ConstLanelets result_lanelets; - while (true) { - RCLCPP_INFO(this->get_logger(), "base lanelet id: %ld", base_lanelet.id()); - result_lanelets.emplace_back(base_lanelet); + result_lanelets.emplace_back(start_lanelet); - // If there are multiple lanes to the left of the current lane, retrieve all of them - const auto left_lanelets = get_all_left_lanelets(base_lanelet); - result_lanelets.insert(result_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + // Update current_lanelet to the next following lanelet, if any + while (route_handler_.getNextLaneletWithinRoute(current_lanelet, ¤t_lanelet)) { + RCLCPP_INFO(this->get_logger(), "current lanelet id: %ld", current_lanelet.id()); - // search next following lanelet - const bool has_next = route_handler_.getNextLaneletWithinRoute(base_lanelet, &next_lanelet); - if (!has_next) { - break; - } - base_lanelet = next_lanelet; + result_lanelets.emplace_back(current_lanelet); + + // Add all left lanelets + auto left_lanelets = get_all_left_lanelets(current_lanelet); + result_lanelets.insert(result_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); } return result_lanelets; } /** - * @brief get all lanes followed by state_lanelet and left adjacent lanes of those lanes. + * @brief * @param base_lanelet * @return */ @@ -204,14 +211,25 @@ lanelet::ConstLanelets MrmPullOverManager::get_all_left_lanelets( } /** - * @brief + * @brief Find the goals that have the same lanelet id with the candidate_lanelets * @param candidate_lanelets * @return */ std::vector MrmPullOverManager::find_goals_in_lanelets( const lanelet::ConstLanelets & candidate_lanelets) const { + RCLCPP_INFO(this->get_logger(), "candidate count: %ld", candidate_lanelets.size()); std::vector goals; + + for (const auto & lane : candidate_lanelets) { + const auto it = candidate_goals_.find(lane.id()); + + // found the goal that has the same lanelet id + if (it != candidate_goals_.end()) { + goals.emplace_back(it->second); + } + } + return goals; } From 34cdca023e56559e671da5e1809eefba627d3c18 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 15:44:44 +0900 Subject: [PATCH 04/48] wip Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.hpp | 59 +++-- .../launch/mrm_pull_over_manager.launch.xml | 4 + system/mrm_pull_over_manager/package.xml | 4 +- .../mrm_pull_over_manager_core.cpp | 250 ++++++++++-------- 4 files changed, 192 insertions(+), 125 deletions(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index f1ca6109bc51b..6bf1264a5d134 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -15,15 +15,24 @@ #ifndef MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ #define MRM_PULL_OVER_MANAGER__MRM_PULL_OVER_MANAGER_CORE_HPP_ -// Core +// C++ #include #include #include +// ROS 2 +#include + +#include +#include + // Autoware +#include #include #include +#include +#include // lanelet #include @@ -37,10 +46,6 @@ #include #include -// ROS 2 core -#include - -#include namespace mrm_pull_over_manager { class MrmPullOverManager : public rclcpp::Node @@ -53,22 +58,33 @@ class MrmPullOverManager : public rclcpp::Node using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; using PoseLaneIdMap = std::map; + using PoseArray = geometry_msgs::msg::PoseArray; + using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; - // Subscribers + // Subscribtoers rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_trajectory_; Odometry::ConstSharedPtr odom_; route_handler::RouteHandler route_handler_; + Trajectory::ConstSharedPtr trajectory_; void on_odometry(const Odometry::ConstSharedPtr msg); void on_route(const LaneletRoute::ConstSharedPtr msg); void on_map(const HADMapBin::ConstSharedPtr msg); + void on_trajectory(const Trajectory::ConstSharedPtr msg); - // Publisher + // Server + rclcpp::Service::SharedPtr activate_pull_over_; - // Clients + void activatePullOver( + const tier4_system_msgs::srv::ActivatePullOver::Request::SharedPtr request, + const tier4_system_msgs::srv::ActivatePullOver::Response::SharedPtr response); + + // Publisher + rclcpp::Publisher::SharedPtr pub_pose_array_; // TODO: temporary for debug // Timer @@ -82,13 +98,26 @@ class MrmPullOverManager : public rclcpp::Node // Algorithm bool is_data_ready(); - std::vector find_near_goals(); - lanelet::ConstLanelet get_current_lanelet(); - lanelet::ConstLanelets get_all_following_and_left_lanelets( - const lanelet::ConstLanelet & start_lanelet) const; - lanelet::ConstLanelets get_all_left_lanelets(const lanelet::ConstLanelet & base_lanelet) const; - std::vector find_goals_in_lanelets( - const lanelet::ConstLanelets & candidate_lanelets) const; + + /** + * @brief Find the goals within the lanelet and publish them + */ + bool find_nearby_goals(); + + /** + * @brief Find the goals that have the same lanelet id with the candidate_lanelets + * @param candidate_lanelets + * @return + */ + PoseArray find_goals_in_lanelets(const lanelet::ConstLanelets & candidate_lanelets) const; + + /** + * @brief Find the goals that have the same lanelet id with the candidate_lanelets + * @param poses Poses to be filtered + * @return Filtered poses + */ + std::vector filter_nearby_goals( + const std::vector & poses); }; } // namespace mrm_pull_over_manager diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml index 5d2854f178ab3..4c984996c2a29 100644 --- a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -2,6 +2,8 @@ + + @@ -12,6 +14,8 @@ + + diff --git a/system/mrm_pull_over_manager/package.xml b/system/mrm_pull_over_manager/package.xml index e58b01f1db923..0e3d0b7cd15cf 100644 --- a/system/mrm_pull_over_manager/package.xml +++ b/system/mrm_pull_over_manager/package.xml @@ -12,10 +12,8 @@ autoware_cmake - autoware_adapi_v1_msgs - autoware_auto_control_msgs autoware_auto_system_msgs - autoware_auto_vehicle_msgs + motion_utils nav_msgs rclcpp std_msgs diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 8fa44a1f5a067..e176d4b510622 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -33,19 +33,76 @@ geometry_msgs::msg::Pose create_pose(const double x, const double y, const doubl namespace lanelet_util { +/** + * @brief Retrieve the current root lanelet based on the provided pose. + * @param route_handler Reference to a route handler that provides lanelet related utilities. + * @param current_pose Current pose used to determine the relevant lanelet. + * @return The lanelet that corresponds to the current pose. + */ +lanelet::ConstLanelet get_current_lanelet( + const route_handler::RouteHandler & route_handler, const geometry_msgs::msg::Pose & current_pose) +{ + lanelet::ConstLanelet current_lanelet; + route_handler.getClosestLaneletWithinRoute(current_pose, ¤t_lanelet); + + return current_lanelet; +} + +/** + * @brief Retrieve all left adjacent lanelets relative to the base lanelet. + * @param route_handler Reference to a route handler that provides lanelet related utilities. + * @param base_lanelet The reference lanelet from which left adjacent lanelets are determined. + * @return A collection of lanelets that are to the left of the base lanelet. + */ +lanelet::ConstLanelets get_all_left_lanelets( + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & base_lanelet) +{ + lanelet::ConstLanelets left_lanalets; + auto left_lanelet = route_handler.getLeftLanelet(base_lanelet, false, true); + while (left_lanelet) { + left_lanalets.emplace_back(left_lanelet.get()); + RCLCPP_INFO(rclcpp::get_logger(__func__), "left lanelet id: %ld", left_lanelet->id()); + + // get next left lanelet + left_lanelet = route_handler.getLeftLanelet(left_lanelet.get(), false, true); + } + + return left_lanalets; +} + +/** + * @brief Retrieve all following lanes and left adjacent lanes starting from the provided start + * lanelet. + * @param route_handler Reference to a route handler that provides lanelet related utilities. + * @param start_lanelet The initial lanelet from which following are determined. + * @return A collection of lanelets that are following and to the left of the start lanelet. + */ +lanelet::ConstLanelets get_all_following_and_left_lanelets( + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & start_lanelet) +{ + lanelet::ConstLanelet current_lanelet = start_lanelet; + lanelet::ConstLanelets result_lanelets; + result_lanelets.emplace_back(start_lanelet); + + // Update current_lanelet to the next following lanelet, if any + while (route_handler.getNextLaneletWithinRoute(current_lanelet, ¤t_lanelet)) { + RCLCPP_INFO(rclcpp::get_logger(__func__), "current lanelet id: %ld", current_lanelet.id()); + + result_lanelets.emplace_back(current_lanelet); + + // Add all left lanelets + auto left_lanelets = get_all_left_lanelets(route_handler, current_lanelet); + result_lanelets.insert(result_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + } + + return result_lanelets; +} } // namespace lanelet_util MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") { // Parameter // param_.update_rate = declare_parameter("update_rate", 10); - // param_.timeout_hazard_status = declare_parameter("timeout_hazard_status", 0.5); - // param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); - // param_.use_takeover_request = declare_parameter("use_takeover_request", false); - // param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); - // param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); - // param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", - // true); using std::placeholders::_1; @@ -58,26 +115,11 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") sub_map_ = create_subscription( "~/input/lanelet_map", rclcpp::QoS{1}.transient_local(), std::bind(&MrmPullOverManager::on_map, this, _1)); + sub_trajectory_ = create_subscription( + "~/input/trajectory", rclcpp::QoS{1}, std::bind(&MrmPullOverManager::on_trajectory, this, _1)); // Publisher - // pub_control_command_ = - // create_publisher( - // "~/output/control_command", rclcpp::QoS{1}); - // pub_hazard_cmd_ = create_publisher( - // "~/output/hazard", rclcpp::QoS{1}); - // pub_gear_cmd_ = - // create_publisher("~/output/gear", - // rclcpp::QoS{1}); - // pub_mrm_state_ = - // create_publisher("~/output/mrm/state", - // rclcpp::QoS{1}); - - // Clients - // client_mrm_comfortable_stop_group_ = - // create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - // client_mrm_comfortable_stop_ = create_client( - // "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, - // client_mrm_comfortable_stop_group_); + pub_pose_array_ = create_publisher("~/output/emergency_goals", rclcpp::QoS{1}); // TODO: temporary // Timer @@ -88,10 +130,24 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") void MrmPullOverManager::on_timer() { - if (is_data_ready()) { - const auto emergency_goals = find_near_goals(); + const bool result = find_nearby_goals(); +} + +void MrmPullOverManager::activatePullOver( + const tier4_system_msgs::srv::ActivatePullOver::Request::SharedPtr request, + const tier4_system_msgs::srv::ActivatePullOver::Response::SharedPtr response) +{ + if (request->activate == true) { + const bool result = find_nearby_goals(); + response->status.success = result; + } + + if (request->activate == false) { + // TODO: Call ClearEmergency service + response->status.success = true; } } + void MrmPullOverManager::on_odometry(const Odometry::ConstSharedPtr msg) { odom_ = msg; @@ -107,6 +163,11 @@ void MrmPullOverManager::on_map(const HADMapBin::ConstSharedPtr msg) route_handler_.setMap(*msg); } +void MrmPullOverManager::on_trajectory(const Trajectory::ConstSharedPtr msg) +{ + trajectory_ = msg; +} + bool MrmPullOverManager::is_data_ready() { if (!odom_) { @@ -122,115 +183,90 @@ bool MrmPullOverManager::is_data_ready() return false; } - return true; -} - -std::vector MrmPullOverManager::find_near_goals() -{ - const auto current_lanelet = get_current_lanelet(); - RCLCPP_INFO(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); - - const auto candidate_lanelets = get_all_following_and_left_lanelets(current_lanelet); - - // candidate_goals_.emplace_back(create_pose_with_lane_id(1, 1, 1, 19464)); - // candidate_goals_.emplace_back(create_pose_with_lane_id(2, 2, 2, 19779)); - // candidate_goals_.emplace_back(create_pose_with_lane_id(3, 3, 3, 20976)); - // candidate_goals_.emplace_back(create_pose_with_lane_id(4, 4, 4, 20478)); - candidate_goals_[19464] = create_pose(1, 1, 1); - candidate_goals_[19779] = create_pose(2, 2, 2); - candidate_goals_[20976] = create_pose(3, 3, 3); - candidate_goals_[20478] = create_pose(4, 4, 4); - const auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); - - RCLCPP_INFO(this->get_logger(), "---found goals---"); - for (const auto & goal : emergency_goals) { - RCLCPP_INFO(this->get_logger(), "goal x: %f", goal.position.x); + if (!trajectory_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for trajectory."); + return false; } - return emergency_goals; + return true; } -/** - * @brief get current root lanelet - * @param - * @return current lanelet - */ -lanelet::ConstLanelet MrmPullOverManager::get_current_lanelet() +bool MrmPullOverManager::find_nearby_goals() { - lanelet::ConstLanelet current_lanelet; - route_handler_.getClosestLaneletWithinRoute(odom_->pose.pose, ¤t_lanelet); + if (!is_data_ready()) { + return false; + } - return current_lanelet; -} + RCLCPP_INFO(this->get_logger(), "--- search lanelets within the route ---"); -/** - * @brief get all following lanes and left adjacent lanes from start_lanelet. - * @param start_lanelet - * @return current lanelet - */ -lanelet::ConstLanelets MrmPullOverManager::get_all_following_and_left_lanelets( - const lanelet::ConstLanelet & start_lanelet) const -{ - lanelet::ConstLanelet current_lanelet = start_lanelet; - lanelet::ConstLanelets result_lanelets; - result_lanelets.emplace_back(start_lanelet); + const auto current_lanelet = lanelet_util::get_current_lanelet(route_handler_, odom_->pose.pose); + RCLCPP_INFO(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); - // Update current_lanelet to the next following lanelet, if any - while (route_handler_.getNextLaneletWithinRoute(current_lanelet, ¤t_lanelet)) { - RCLCPP_INFO(this->get_logger(), "current lanelet id: %ld", current_lanelet.id()); + const auto candidate_lanelets = + lanelet_util::get_all_following_and_left_lanelets(route_handler_, current_lanelet); - result_lanelets.emplace_back(current_lanelet); + candidate_goals_[19464] = create_pose(65401.7, 684.4, 715.8); + candidate_goals_[19779] = create_pose(65567.9, 681.5, 714.5); + candidate_goals_[20976] = create_pose(65808.1, 720.5, 712.6); + candidate_goals_[20478] = create_pose(65570.5, 673.9, 714.5); + auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); - // Add all left lanelets - auto left_lanelets = get_all_left_lanelets(current_lanelet); - result_lanelets.insert(result_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + RCLCPP_INFO(this->get_logger(), "---found goals---"); + for (const auto & goal : emergency_goals.poses) { + RCLCPP_INFO(this->get_logger(), "goal x: %f", goal.position.x); } - return result_lanelets; -} + emergency_goals.poses = filter_nearby_goals(emergency_goals.poses); -/** - * @brief - * @param base_lanelet - * @return - */ -lanelet::ConstLanelets MrmPullOverManager::get_all_left_lanelets( - const lanelet::ConstLanelet & base_lanelet) const -{ - lanelet::ConstLanelets left_lanalets; - auto left_lanelet = route_handler_.getLeftLanelet(base_lanelet, false, true); - while (left_lanelet) { - left_lanalets.emplace_back(left_lanelet.get()); - RCLCPP_INFO(this->get_logger(), "left lanelet id: %ld", left_lanelet->id()); + pub_pose_array_->publish(emergency_goals); - // get next left lanelet - left_lanelet = route_handler_.getLeftLanelet(left_lanelet.get(), false, true); - } - - return left_lanalets; + return true; } -/** - * @brief Find the goals that have the same lanelet id with the candidate_lanelets - * @param candidate_lanelets - * @return - */ -std::vector MrmPullOverManager::find_goals_in_lanelets( +geometry_msgs::msg::PoseArray MrmPullOverManager::find_goals_in_lanelets( const lanelet::ConstLanelets & candidate_lanelets) const { RCLCPP_INFO(this->get_logger(), "candidate count: %ld", candidate_lanelets.size()); - std::vector goals; + PoseArray goals; + goals.header.frame_id = "map"; + goals.header.stamp = now(); // TODO for (const auto & lane : candidate_lanelets) { const auto it = candidate_goals_.find(lane.id()); // found the goal that has the same lanelet id if (it != candidate_goals_.end()) { - goals.emplace_back(it->second); + goals.poses.emplace_back(it->second); } } return goals; } +std::vector MrmPullOverManager::filter_nearby_goals( + const std::vector & poses) +{ + RCLCPP_INFO(this->get_logger(), "pose count: %ld", poses.size()); + std::vector filtered_poses; + + // The poses are stored in order of distance, so the loop exits as soon as the first one exceeding + // the threshold is found + const double distance_threshold = 10.0; // TODO + auto it = poses.begin(); + for (; it != poses.end(); ++it) { + const double arc_length_to_pose = motion_utils::calcSignedArcLength( + trajectory_->points, odom_->pose.pose.position, it->position); + RCLCPP_INFO(this->get_logger(), "distance to the pose: %lf", arc_length_to_pose); + if (arc_length_to_pose > distance_threshold) { + break; + } + } + + // Store poses that have larger distance than threshold + if (it != poses.end()) { + filtered_poses.insert(filtered_poses.end(), it, poses.end()); + } + + return filtered_poses; +} } // namespace mrm_pull_over_manager From 4872082223f85744fc2d7ac32f7b718a4e375539 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 17:30:10 +0900 Subject: [PATCH 05/48] wip Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager/csv_parser.hpp | 103 ++++++++++++++++++ .../mrm_pull_over_manager_core.hpp | 14 +-- .../mrm_pull_over_manager_core.cpp | 83 +++++++++----- 3 files changed, 164 insertions(+), 36 deletions(-) create mode 100644 system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp new file mode 100644 index 0000000000000..e19fa44ead7f4 --- /dev/null +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp @@ -0,0 +1,103 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MRM_PULL_OVER_MANAGER__CSV_PARSER_HPP_ +#define MRM_PULL_OVER_MANAGER__CSV_PARSER_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace mrm_pull_over_manager +{ +class CsvPoseParser +{ +public: + CsvPoseParser(const std::string & filepath) : filepath_(filepath) {} + + std::map parse() + { + std::map pose_id_map; + std::ifstream file(filepath_); + + if (!file.is_open()) { + std::cerr << "Failed to open the CSV file." << std::endl; + return pose_id_map; + } + + // Skip header line + std::string line; + std::getline(file, line); + + while (std::getline(file, line)) { + std::istringstream ss(line); + std::string field; + double x, y, z, yaw; + int lane_id; // Not used in this example + + std::getline(ss, field, ','); + x = std::stod(field); + + std::getline(ss, field, ','); + y = std::stod(field); + + std::getline(ss, field, ','); + z = std::stod(field); + + std::getline(ss, field, ','); + yaw = std::stod(field); + + std::getline(ss, field, ','); + lane_id = std::stoi(field); + + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + yaw_to_quaternion(yaw, pose.orientation); + + pose_id_map[lane_id] = pose; + } + + return pose_id_map; + } + +private: + std::string filepath_; + + void yaw_to_quaternion(double yaw, geometry_msgs::msg::Quaternion & quaternion) + { + quaternion.z = sin(yaw * 0.5); + quaternion.w = cos(yaw * 0.5); + } +}; + +int main() +{ + const std::string filepath = "path_to_your_csv.csv"; + CsvPoseParser parser(filepath); + auto poses = parser.parse(); + // ... Use the poses as needed + return 0; +} +} // namespace mrm_pull_over_manager + +#endif // MRM_PULL_OVER_MANAGER__CSV_PARSER_HPP_ \ No newline at end of file diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 6bf1264a5d134..6d31c19b3d4f3 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -54,12 +54,13 @@ class MrmPullOverManager : public rclcpp::Node MrmPullOverManager(); private: + using Pose = geometry_msgs::msg::Pose; + using PoseArray = geometry_msgs::msg::PoseArray; + using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using Odometry = nav_msgs::msg::Odometry; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using PoseLaneIdMap = std::map; - using PoseArray = geometry_msgs::msg::PoseArray; - using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; + using PoseLaneIdMap = std::map; // Subscribtoers rclcpp::Subscription::SharedPtr sub_odom_; @@ -102,22 +103,21 @@ class MrmPullOverManager : public rclcpp::Node /** * @brief Find the goals within the lanelet and publish them */ - bool find_nearby_goals(); + bool find_goals_within_route(); /** * @brief Find the goals that have the same lanelet id with the candidate_lanelets * @param candidate_lanelets * @return */ - PoseArray find_goals_in_lanelets(const lanelet::ConstLanelets & candidate_lanelets) const; + std::vector find_goals_in_lanelets(const lanelet::ConstLanelets & candidate_lanelets) const; /** * @brief Find the goals that have the same lanelet id with the candidate_lanelets * @param poses Poses to be filtered * @return Filtered poses */ - std::vector filter_nearby_goals( - const std::vector & poses); + std::vector filter_nearby_goals(const std::vector & poses); }; } // namespace mrm_pull_over_manager diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index e176d4b510622..89056e39eabfd 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -14,21 +14,37 @@ #include "mrm_pull_over_manager/mrm_pull_over_manager_core.hpp" +#include "mrm_pull_over_manager/csv_parser.hpp" namespace mrm_pull_over_manager { -// TODO: あとでけす +// TODO: temporary namespace { -geometry_msgs::msg::Pose create_pose(const double x, const double y, const double z) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; - - return pose; -} +// geometry_msgs::msg::Quaternion yaw_to_quaternion(double yaw) +// { +// geometry_msgs::msg::Quaternion ros_quat; + +// // Convert yaw angle (theta) to quaternion +// ros_quat.x = 0.0; +// ros_quat.y = 0.0; +// ros_quat.z = sin(yaw * 0.5); +// ros_quat.w = cos(yaw * 0.5); + +// return ros_quat; +// } + +// geometry_msgs::msg::Pose create_pose( +// const double x, const double y, const double z, const double yaw) +// { +// geometry_msgs::msg::Pose pose; +// pose.position.x = x; +// pose.position.y = y; +// pose.position.z = z; +// pose.orientation = yaw_to_quaternion(yaw); + +// return pose; +// } } // namespace namespace lanelet_util @@ -121,6 +137,10 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") // Publisher pub_pose_array_ = create_publisher("~/output/emergency_goals", rclcpp::QoS{1}); + const std::string filepath = "/media/tomohitoando/data/map/shiojiri_100laps/pull_over_point.csv"; + CsvPoseParser parser(filepath); + candidate_goals_ = parser.parse(); + // TODO: temporary // Timer const auto update_period_ns = rclcpp::Rate(10.0).period(); @@ -130,7 +150,8 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") void MrmPullOverManager::on_timer() { - const bool result = find_nearby_goals(); + const bool result = find_goals_within_route(); + RCLCPP_INFO_STREAM(this->get_logger(), "result: " << result); } void MrmPullOverManager::activatePullOver( @@ -138,7 +159,7 @@ void MrmPullOverManager::activatePullOver( const tier4_system_msgs::srv::ActivatePullOver::Response::SharedPtr response) { if (request->activate == true) { - const bool result = find_nearby_goals(); + const bool result = find_goals_within_route(); response->status.success = result; } @@ -191,27 +212,24 @@ bool MrmPullOverManager::is_data_ready() return true; } -bool MrmPullOverManager::find_nearby_goals() +bool MrmPullOverManager::find_goals_within_route() { if (!is_data_ready()) { return false; } - RCLCPP_INFO(this->get_logger(), "--- search lanelets within the route ---"); - const auto current_lanelet = lanelet_util::get_current_lanelet(route_handler_, odom_->pose.pose); RCLCPP_INFO(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); const auto candidate_lanelets = lanelet_util::get_all_following_and_left_lanelets(route_handler_, current_lanelet); - candidate_goals_[19464] = create_pose(65401.7, 684.4, 715.8); - candidate_goals_[19779] = create_pose(65567.9, 681.5, 714.5); - candidate_goals_[20976] = create_pose(65808.1, 720.5, 712.6); - candidate_goals_[20478] = create_pose(65570.5, 673.9, 714.5); - auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); + PoseArray emergency_goals; + emergency_goals.header.frame_id = "map"; + emergency_goals.header.stamp = now(); + emergency_goals.poses = find_goals_in_lanelets(candidate_lanelets); - RCLCPP_INFO(this->get_logger(), "---found goals---"); + // temporary for (const auto & goal : emergency_goals.poses) { RCLCPP_INFO(this->get_logger(), "goal x: %f", goal.position.x); } @@ -223,20 +241,18 @@ bool MrmPullOverManager::find_nearby_goals() return true; } -geometry_msgs::msg::PoseArray MrmPullOverManager::find_goals_in_lanelets( +std::vector MrmPullOverManager::find_goals_in_lanelets( const lanelet::ConstLanelets & candidate_lanelets) const { - RCLCPP_INFO(this->get_logger(), "candidate count: %ld", candidate_lanelets.size()); - PoseArray goals; - goals.header.frame_id = "map"; - goals.header.stamp = now(); // TODO + RCLCPP_INFO(this->get_logger(), "candidate lanelet count: %ld", candidate_lanelets.size()); + std::vector goals; for (const auto & lane : candidate_lanelets) { const auto it = candidate_goals_.find(lane.id()); // found the goal that has the same lanelet id if (it != candidate_goals_.end()) { - goals.poses.emplace_back(it->second); + goals.emplace_back(it->second); } } @@ -249,11 +265,20 @@ std::vector MrmPullOverManager::filter_nearby_goals( RCLCPP_INFO(this->get_logger(), "pose count: %ld", poses.size()); std::vector filtered_poses; - // The poses are stored in order of distance, so the loop exits as soon as the first one exceeding - // the threshold is found const double distance_threshold = 10.0; // TODO + const double yaw_threshold = 1.0; // TODO auto it = poses.begin(); for (; it != poses.end(); ++it) { + // filter unsafe yaw pose + const double yaw_deviation = motion_utils::calcYawDeviation(trajectory_->points, *it); + RCLCPP_INFO(this->get_logger(), "yaw deviation to pose: %lf", yaw_deviation); + if (std::abs(yaw_deviation) > yaw_threshold) { + continue; + } + + // filter too near pose + // The poses are stored in order of distance, so the loop exits as soon as the first one + // exceeding the threshold is found const double arc_length_to_pose = motion_utils::calcSignedArcLength( trajectory_->points, odom_->pose.pose.position, it->position); RCLCPP_INFO(this->get_logger(), "distance to the pose: %lf", arc_length_to_pose); From c5e4b4c300843bcf436e6935607a90f8e67a9c9e Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Sun, 15 Oct 2023 19:27:21 +0900 Subject: [PATCH 06/48] feat(emergency_goal_manager): add an emergency_goal_manager package Signed-off-by: Makoto Kurihara --- system/emergency_goal_manager/CMakeLists.txt | 18 +++++ system/emergency_goal_manager/README.md | 39 ++++++++++ .../config/emergency_goal_manager.param.yaml | 5 ++ .../emergency_goal_manager_node.hpp | 61 ++++++++++++++++ .../launch/emergency_goal_manager.launch.xml | 71 +++++++++++++++++++ system/emergency_goal_manager/package.xml | 24 +++++++ .../src/emergency_goal_manager_node.cpp | 43 +++++++++++ 7 files changed, 261 insertions(+) create mode 100644 system/emergency_goal_manager/CMakeLists.txt create mode 100644 system/emergency_goal_manager/README.md create mode 100644 system/emergency_goal_manager/config/emergency_goal_manager.param.yaml create mode 100644 system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp create mode 100644 system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml create mode 100644 system/emergency_goal_manager/package.xml create mode 100644 system/emergency_goal_manager/src/emergency_goal_manager_node.cpp diff --git a/system/emergency_goal_manager/CMakeLists.txt b/system/emergency_goal_manager/CMakeLists.txt new file mode 100644 index 0000000000000..281cf2eeb53d5 --- /dev/null +++ b/system/emergency_goal_manager/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(emergency_goal_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(emergency_goal_manager_component SHARED + src/emergency_goal_manager_node.cpp +) + +rclcpp_components_register_node(emergency_goal_manager_component + PLUGIN "emergency_goal_manager::EmergencyGoalManager" + EXECUTABLE emergency_goal_manager) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/emergency_goal_manager/README.md b/system/emergency_goal_manager/README.md new file mode 100644 index 0000000000000..bf6a46ac8c841 --- /dev/null +++ b/system/emergency_goal_manager/README.md @@ -0,0 +1,39 @@ +# emergency_goal_manager + +## Purpose + +The Emergency goal manager is responsible for coordinating the goal poses for emergency rerouting and communicating it to the mission planner. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------ | ---------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| | | | + +### Output + +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------------------- | ---------------------- | +| | | | + +## Parameters + +### Node Parameters + +| Name | Type | Default value | Explanation | +| ----------- | ---- | ------------- | ----------------------------- | +| | | | | + +### Core Parameters + +| Name | Type | Default value | Explanation | +| ------------------- | ------ | ------------- | ---------------------------------------------- | +| | | | | + +## Assumptions / Known limits + +TBD. diff --git a/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml new file mode 100644 index 0000000000000..1ee2699a23a82 --- /dev/null +++ b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + update_rate: 30 + target_acceleration: -2.5 + target_jerk: -1.5 diff --git a/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp b/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp new file mode 100644 index 0000000000000..a763d2e1a5a23 --- /dev/null +++ b/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp @@ -0,0 +1,61 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ +#define EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ + +// Core +#include +#include + +// Autoware + +// ROS 2 core +#include + +namespace emergency_goal_manager +{ + +struct Parameters +{ + int update_rate; // [Hz] + double target_acceleration; // [m/s^2] + double target_jerk; // [m/s^3] +}; + +class EmergencyGoalManagerNode : public rclcpp::Node +{ +public: + explicit EmergencyGoalManagerNode(const rclcpp::NodeOptions & node_options); + +private: + // Parameters + Parameters params_; + + // Subscriber + + // Server + + // Publisher + + // Timer + + // States + + // Algorithm +}; + +} // namespace emergency_goal_manager + +#endif // EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ diff --git a/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml new file mode 100644 index 0000000000000..769c121c18ac9 --- /dev/null +++ b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml @@ -0,0 +1,71 @@ +# 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. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + config_file_path = LaunchConfiguration("config_file").perform(context) + with open(config_file_path, "r") as f: + params = yaml.safe_load(f)["/**"]["ros__parameters"] + + component = ComposableNode( + package="mrm_emergency_stop_operator", + plugin="mrm_emergency_stop_operator::MrmEmergencyStopOperator", + name="mrm_emergency_stop_operator", + parameters=[ + params, + ], + remappings=[ + ("~/input/mrm/emergency_stop/operate", "/system/mrm/emergency_stop/operate"), + ("~/input/control/control_cmd", "/control/command/control_cmd"), + ("~/output/mrm/emergency_stop/status", "/system/mrm/emergency_stop/status"), + ("~/output/mrm/emergency_stop/control_cmd", "/system/emergency/control_cmd"), + ], + ) + + container = ComposableNodeContainer( + name="mrm_emergency_stop_operator_container", + namespace="mrm_emergency_stop_operator", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + component, + ], + output="screen", + ) + + return [container] + + +def generate_launch_description(): + launch_arguments = [ + DeclareLaunchArgument( + "config_file", + default_value=[ + FindPackageShare("mrm_emergency_stop_operator"), + "/config/mrm_emergency_stop_operator.param.yaml", + ], + description="path to the parameter file of mrm_emergency_stop_operator", + ) + ] + + return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/system/emergency_goal_manager/package.xml b/system/emergency_goal_manager/package.xml new file mode 100644 index 0000000000000..150855de06b39 --- /dev/null +++ b/system/emergency_goal_manager/package.xml @@ -0,0 +1,24 @@ + + + + emergency_goal_manager + 0.1.0 + The emergency goal manager package + Makoto Kurihara + Tomohito Ando + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + rclcpp_components + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp new file mode 100644 index 0000000000000..00e017d0c4680 --- /dev/null +++ b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp @@ -0,0 +1,43 @@ +// 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 + +namespace emergency_goal_manager +{ + +EmergencyGoalManagerNode::EmergencyGoalManagerNode(const rclcpp::NodeOptions & node_options) +: Node("emergency_goal_manager_node", node_options) +{ + // Parameter + params_.update_rate = static_cast(declare_parameter("update_rate", 30)); + params_.target_acceleration = declare_parameter("target_acceleration", -2.5); + params_.target_jerk = declare_parameter("target_jerk", -1.5); + + // Subscriber + + // Server + + // Publisher + + // Timer + + // Initialize +} + + +} // namespace emergency_goal_manager + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(emergency_goal_manager::EmergencyGoalManagerNode) From fa81555998fa98f811eed73934bb7df3cb8372c9 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 21:08:13 +0900 Subject: [PATCH 07/48] feat: add mrm_pull_over_manager Signed-off-by: Tomohito Ando --- .../config/mrm_pull_over_manager.param.yaml | 6 +- .../config/pull_over_point.csv | 5 + .../mrm_pull_over_manager/csv_parser.hpp | 2 +- .../mrm_pull_over_manager_core.hpp | 19 ++- .../launch/mrm_pull_over_manager.launch.xml | 16 ++- .../mrm_pull_over_manager_core.cpp | 124 ++++++++++-------- 6 files changed, 105 insertions(+), 67 deletions(-) create mode 100644 system/mrm_pull_over_manager/config/pull_over_point.csv diff --git a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml index 799fdc39ad5b8..afb7b6b4fc3f9 100644 --- a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml +++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml @@ -1,5 +1,5 @@ -# Default configuration for emergency handler ---- /**: ros__parameters: - test: 10 + max_goal_pose_num: 3 + yaw_deviation_threshold: 0.5 # [rad] + distance_threshold: 10.0 # [m] diff --git a/system/mrm_pull_over_manager/config/pull_over_point.csv b/system/mrm_pull_over_manager/config/pull_over_point.csv new file mode 100644 index 0000000000000..8e6a50d36f196 --- /dev/null +++ b/system/mrm_pull_over_manager/config/pull_over_point.csv @@ -0,0 +1,5 @@ +x,y,z,yaw,lane_id +65401.7, 684.4, 715.8, 0, 19464 +65567.9, 681.5, 714.5, 0, 19779 +65808.1, 720.5, 712.6, -1.3, 20976 +65570.5, 673.9, 714.5, 3.14, 20478 diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp index e19fa44ead7f4..7ee47ac036012 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp @@ -31,7 +31,7 @@ namespace mrm_pull_over_manager class CsvPoseParser { public: - CsvPoseParser(const std::string & filepath) : filepath_(filepath) {} + explicit CsvPoseParser(const std::string & filepath) : filepath_(filepath) {} std::map parse() { diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 6d31c19b3d4f3..c16711e92f9c4 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -18,6 +18,7 @@ // C++ #include #include +#include #include // ROS 2 @@ -32,6 +33,7 @@ #include #include +#include #include // lanelet @@ -61,6 +63,15 @@ class MrmPullOverManager : public rclcpp::Node using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; using PoseLaneIdMap = std::map; + using MrmBehaviorStatus = tier4_system_msgs::msg::MrmBehaviorStatus; + + struct Parameter + { + std::string pull_over_point_file_path; + size_t max_goal_pose_num; + double yaw_deviation_threshold; + double distance_threshold; + }; // Subscribtoers rclcpp::Subscription::SharedPtr sub_odom_; @@ -69,6 +80,7 @@ class MrmPullOverManager : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_trajectory_; Odometry::ConstSharedPtr odom_; + LaneletRoute::ConstSharedPtr route_; route_handler::RouteHandler route_handler_; Trajectory::ConstSharedPtr trajectory_; @@ -86,19 +98,22 @@ class MrmPullOverManager : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_pose_array_; + rclcpp::Publisher::SharedPtr pub_status_; - // TODO: temporary for debug // Timer rclcpp::TimerBase::SharedPtr timer_; void on_timer(); // Parameters - // Param param_; + Parameter param_; + // Variables PoseLaneIdMap candidate_goals_; + MrmBehaviorStatus status_; // Algorithm bool is_data_ready(); + void publishStatus() const; /** * @brief Find the goals within the lanelet and publish them diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml index 4c984996c2a29..e7526153e1b76 100644 --- a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -3,10 +3,11 @@ - - - + + + + @@ -15,10 +16,11 @@ - - - - + + + +i + diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 89056e39eabfd..24e380a78f916 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -17,36 +17,6 @@ #include "mrm_pull_over_manager/csv_parser.hpp" namespace mrm_pull_over_manager { - -// TODO: temporary -namespace -{ -// geometry_msgs::msg::Quaternion yaw_to_quaternion(double yaw) -// { -// geometry_msgs::msg::Quaternion ros_quat; - -// // Convert yaw angle (theta) to quaternion -// ros_quat.x = 0.0; -// ros_quat.y = 0.0; -// ros_quat.z = sin(yaw * 0.5); -// ros_quat.w = cos(yaw * 0.5); - -// return ros_quat; -// } - -// geometry_msgs::msg::Pose create_pose( -// const double x, const double y, const double z, const double yaw) -// { -// geometry_msgs::msg::Pose pose; -// pose.position.x = x; -// pose.position.y = y; -// pose.position.z = z; -// pose.orientation = yaw_to_quaternion(yaw); - -// return pose; -// } -} // namespace - namespace lanelet_util { /** @@ -94,21 +64,35 @@ lanelet::ConstLanelets get_all_left_lanelets( * @return A collection of lanelets that are following and to the left of the start lanelet. */ lanelet::ConstLanelets get_all_following_and_left_lanelets( - const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & start_lanelet) + const route_handler::RouteHandler & route_handler, + const autoware_planning_msgs::msg::LaneletRoute & route, + const lanelet::ConstLanelet & start_lanelet) { - lanelet::ConstLanelet current_lanelet = start_lanelet; lanelet::ConstLanelets result_lanelets; - result_lanelets.emplace_back(start_lanelet); + bool found_start_lane = false; + for (const auto & segment : route.segments) { + // If the start lanelet hasn't been found, search for it + if (!found_start_lane) { + for (const auto & primitive : segment.primitives) { + if (primitive.id == start_lanelet.id()) { + found_start_lane = true; + break; // Break out once the start lanelet is found + } + } + } - // Update current_lanelet to the next following lanelet, if any - while (route_handler.getNextLaneletWithinRoute(current_lanelet, ¤t_lanelet)) { - RCLCPP_INFO(rclcpp::get_logger(__func__), "current lanelet id: %ld", current_lanelet.id()); + // search lanelets from the start_lanelet + if (!found_start_lane) { + continue; + } - result_lanelets.emplace_back(current_lanelet); + const auto current_lane = route_handler.getLaneletsFromId(segment.preferred_primitive.id); + result_lanelets.emplace_back(current_lane); + RCLCPP_INFO(rclcpp::get_logger(__func__), "current lanelet id: %ld", current_lane.id()); // Add all left lanelets - auto left_lanelets = get_all_left_lanelets(route_handler, current_lanelet); - result_lanelets.insert(result_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + auto left_lanes = get_all_left_lanelets(route_handler, current_lane); + result_lanelets.insert(result_lanelets.end(), left_lanes.begin(), left_lanes.end()); } return result_lanelets; @@ -118,9 +102,13 @@ lanelet::ConstLanelets get_all_following_and_left_lanelets( MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") { // Parameter - // param_.update_rate = declare_parameter("update_rate", 10); + param_.pull_over_point_file_path = declare_parameter("pull_over_point_file_path"); + param_.max_goal_pose_num = declare_parameter("max_goal_pose_num"); + param_.yaw_deviation_threshold = declare_parameter("yaw_deviation_threshold"); + param_.distance_threshold = declare_parameter("distance_threshold"); using std::placeholders::_1; + using std::placeholders::_2; // Subscriber sub_odom_ = create_subscription( @@ -135,23 +123,38 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") "~/input/trajectory", rclcpp::QoS{1}, std::bind(&MrmPullOverManager::on_trajectory, this, _1)); // Publisher - pub_pose_array_ = create_publisher("~/output/emergency_goals", rclcpp::QoS{1}); + pub_pose_array_ = + create_publisher("~/output/mrm/pull_over/emergency_goals", rclcpp::QoS{1}); + pub_status_ = + create_publisher("~/output/mrm/pull_over/status", rclcpp::QoS{1}); - const std::string filepath = "/media/tomohitoando/data/map/shiojiri_100laps/pull_over_point.csv"; - CsvPoseParser parser(filepath); - candidate_goals_ = parser.parse(); + // Server + activate_pull_over_ = create_service( + "~/input/mrm/pull_over/activate", + std::bind(&MrmPullOverManager::activatePullOver, this, _1, _2)); - // TODO: temporary - // Timer const auto update_period_ns = rclcpp::Rate(10.0).period(); timer_ = rclcpp::create_timer( this, get_clock(), update_period_ns, std::bind(&MrmPullOverManager::on_timer, this)); + + // Parse CSV and store the emergency goals + CsvPoseParser parser(param_.pull_over_point_file_path); + candidate_goals_ = parser.parse(); + + // Initialize the state + status_.state = MrmBehaviorStatus::AVAILABLE; } void MrmPullOverManager::on_timer() { - const bool result = find_goals_within_route(); - RCLCPP_INFO_STREAM(this->get_logger(), "result: " << result); + publishStatus(); +} + +void MrmPullOverManager::publishStatus() const +{ + auto status = status_; + status.stamp = this->now(); + pub_status_->publish(status); } void MrmPullOverManager::activatePullOver( @@ -161,11 +164,13 @@ void MrmPullOverManager::activatePullOver( if (request->activate == true) { const bool result = find_goals_within_route(); response->status.success = result; + status_.state = MrmBehaviorStatus::OPERATING; } if (request->activate == false) { - // TODO: Call ClearEmergency service + // TODO(TomohitoAndo): Call ClearEmergency service response->status.success = true; + status_.state = MrmBehaviorStatus::AVAILABLE; } } @@ -176,6 +181,8 @@ void MrmPullOverManager::on_odometry(const Odometry::ConstSharedPtr msg) void MrmPullOverManager::on_route(const LaneletRoute::ConstSharedPtr msg) { + // TODO(TomohitoAndo): If we can get all the lanes including lane change, route_ is not required + route_ = msg; route_handler_.setRoute(*msg); } @@ -198,6 +205,13 @@ bool MrmPullOverManager::is_data_ready() return false; } + if (!route_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for route..."); + return false; + } + if (!route_handler_.isHandlerReady()) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "Waiting for route handler."); @@ -222,7 +236,7 @@ bool MrmPullOverManager::find_goals_within_route() RCLCPP_INFO(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); const auto candidate_lanelets = - lanelet_util::get_all_following_and_left_lanelets(route_handler_, current_lanelet); + lanelet_util::get_all_following_and_left_lanelets(route_handler_, *route_, current_lanelet); PoseArray emergency_goals; emergency_goals.header.frame_id = "map"; @@ -254,6 +268,10 @@ std::vector MrmPullOverManager::find_goals_in_lanelets if (it != candidate_goals_.end()) { goals.emplace_back(it->second); } + + if (goals.size() > param_.max_goal_pose_num) { + break; + } } return goals; @@ -265,14 +283,12 @@ std::vector MrmPullOverManager::filter_nearby_goals( RCLCPP_INFO(this->get_logger(), "pose count: %ld", poses.size()); std::vector filtered_poses; - const double distance_threshold = 10.0; // TODO - const double yaw_threshold = 1.0; // TODO auto it = poses.begin(); for (; it != poses.end(); ++it) { // filter unsafe yaw pose const double yaw_deviation = motion_utils::calcYawDeviation(trajectory_->points, *it); RCLCPP_INFO(this->get_logger(), "yaw deviation to pose: %lf", yaw_deviation); - if (std::abs(yaw_deviation) > yaw_threshold) { + if (std::abs(yaw_deviation) > param_.yaw_deviation_threshold) { continue; } @@ -282,7 +298,7 @@ std::vector MrmPullOverManager::filter_nearby_goals( const double arc_length_to_pose = motion_utils::calcSignedArcLength( trajectory_->points, odom_->pose.pose.position, it->position); RCLCPP_INFO(this->get_logger(), "distance to the pose: %lf", arc_length_to_pose); - if (arc_length_to_pose > distance_threshold) { + if (arc_length_to_pose > param_.distance_threshold) { break; } } From 171d4aa6510e16e59adb1e6b0ea9748982c73022 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 21:10:34 +0900 Subject: [PATCH 08/48] change update rate to param Signed-off-by: Tomohito Ando --- .../config/mrm_pull_over_manager.param.yaml | 1 + .../mrm_pull_over_manager/mrm_pull_over_manager_core.hpp | 1 + .../src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml index afb7b6b4fc3f9..7056a1eb8bbe2 100644 --- a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml +++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + update_rate: 10 max_goal_pose_num: 3 yaw_deviation_threshold: 0.5 # [rad] distance_threshold: 10.0 # [m] diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index c16711e92f9c4..057d750c1d4ce 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -67,6 +67,7 @@ class MrmPullOverManager : public rclcpp::Node struct Parameter { + double update_rate; std::string pull_over_point_file_path; size_t max_goal_pose_num; double yaw_deviation_threshold; diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 24e380a78f916..ab180418b86ac 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -102,6 +102,7 @@ lanelet::ConstLanelets get_all_following_and_left_lanelets( MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") { // Parameter + param_.update_rate = declare_parameter("update_rate"); param_.pull_over_point_file_path = declare_parameter("pull_over_point_file_path"); param_.max_goal_pose_num = declare_parameter("max_goal_pose_num"); param_.yaw_deviation_threshold = declare_parameter("yaw_deviation_threshold"); @@ -133,7 +134,8 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") "~/input/mrm/pull_over/activate", std::bind(&MrmPullOverManager::activatePullOver, this, _1, _2)); - const auto update_period_ns = rclcpp::Rate(10.0).period(); + // Timer + const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), update_period_ns, std::bind(&MrmPullOverManager::on_timer, this)); From 08fe546e6f921a2fbee65ff6ed2e7e74bb70e3dc Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 21:24:05 +0900 Subject: [PATCH 09/48] fix Signed-off-by: Tomohito Ando --- .../config/mrm_pull_over_manager.param.yaml | 4 ++-- .../mrm_pull_over_manager/mrm_pull_over_manager_core.hpp | 2 +- .../mrm_pull_over_manager/mrm_pull_over_manager_core.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml index 7056a1eb8bbe2..1d988cc09ece8 100644 --- a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml +++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - update_rate: 10 + update_rate: 10.0 max_goal_pose_num: 3 yaw_deviation_threshold: 0.5 # [rad] - distance_threshold: 10.0 # [m] + margin_time_to_goal: 10.0 # [sec] diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 057d750c1d4ce..4d36a77fe98fd 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -71,7 +71,7 @@ class MrmPullOverManager : public rclcpp::Node std::string pull_over_point_file_path; size_t max_goal_pose_num; double yaw_deviation_threshold; - double distance_threshold; + double margin_time_to_goal; }; // Subscribtoers diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index ab180418b86ac..d427aef8e20a2 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -106,7 +106,7 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") param_.pull_over_point_file_path = declare_parameter("pull_over_point_file_path"); param_.max_goal_pose_num = declare_parameter("max_goal_pose_num"); param_.yaw_deviation_threshold = declare_parameter("yaw_deviation_threshold"); - param_.distance_threshold = declare_parameter("distance_threshold"); + param_.margin_time_to_goal = declare_parameter("margin_time_to_goal"); using std::placeholders::_1; using std::placeholders::_2; @@ -299,8 +299,10 @@ std::vector MrmPullOverManager::filter_nearby_goals( // exceeding the threshold is found const double arc_length_to_pose = motion_utils::calcSignedArcLength( trajectory_->points, odom_->pose.pose.position, it->position); + const double distance_threshold = odom_->twist.twist.linear.x * param_.margin_time_to_goal; RCLCPP_INFO(this->get_logger(), "distance to the pose: %lf", arc_length_to_pose); - if (arc_length_to_pose > param_.distance_threshold) { + RCLCPP_INFO(this->get_logger(), "distance threshold: %lf", distance_threshold); + if (arc_length_to_pose > distance_threshold) { break; } } From 542c80d153c25da31f57aa4991101836b97f3677 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Sun, 15 Oct 2023 21:34:58 +0900 Subject: [PATCH 10/48] change INFO to DEBUG Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.cpp | 26 ++++++++----------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index d427aef8e20a2..e98c78089763f 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -47,7 +47,7 @@ lanelet::ConstLanelets get_all_left_lanelets( auto left_lanelet = route_handler.getLeftLanelet(base_lanelet, false, true); while (left_lanelet) { left_lanalets.emplace_back(left_lanelet.get()); - RCLCPP_INFO(rclcpp::get_logger(__func__), "left lanelet id: %ld", left_lanelet->id()); + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "left lanelet id: %ld", left_lanelet->id()); // get next left lanelet left_lanelet = route_handler.getLeftLanelet(left_lanelet.get(), false, true); @@ -88,7 +88,7 @@ lanelet::ConstLanelets get_all_following_and_left_lanelets( const auto current_lane = route_handler.getLaneletsFromId(segment.preferred_primitive.id); result_lanelets.emplace_back(current_lane); - RCLCPP_INFO(rclcpp::get_logger(__func__), "current lanelet id: %ld", current_lane.id()); + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "current lanelet id: %ld", current_lane.id()); // Add all left lanelets auto left_lanes = get_all_left_lanelets(route_handler, current_lane); @@ -201,14 +201,14 @@ void MrmPullOverManager::on_trajectory(const Trajectory::ConstSharedPtr msg) bool MrmPullOverManager::is_data_ready() { if (!odom_) { - RCLCPP_INFO_THROTTLE( + RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for odometry msg..."); return false; } if (!route_) { - RCLCPP_INFO_THROTTLE( + RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for route..."); return false; @@ -235,7 +235,7 @@ bool MrmPullOverManager::find_goals_within_route() } const auto current_lanelet = lanelet_util::get_current_lanelet(route_handler_, odom_->pose.pose); - RCLCPP_INFO(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); + RCLCPP_DEBUG(this->get_logger(), "current lanelet id:%ld", current_lanelet.id()); const auto candidate_lanelets = lanelet_util::get_all_following_and_left_lanelets(route_handler_, *route_, current_lanelet); @@ -245,11 +245,6 @@ bool MrmPullOverManager::find_goals_within_route() emergency_goals.header.stamp = now(); emergency_goals.poses = find_goals_in_lanelets(candidate_lanelets); - // temporary - for (const auto & goal : emergency_goals.poses) { - RCLCPP_INFO(this->get_logger(), "goal x: %f", goal.position.x); - } - emergency_goals.poses = filter_nearby_goals(emergency_goals.poses); pub_pose_array_->publish(emergency_goals); @@ -260,7 +255,7 @@ bool MrmPullOverManager::find_goals_within_route() std::vector MrmPullOverManager::find_goals_in_lanelets( const lanelet::ConstLanelets & candidate_lanelets) const { - RCLCPP_INFO(this->get_logger(), "candidate lanelet count: %ld", candidate_lanelets.size()); + RCLCPP_DEBUG(this->get_logger(), "candidate lanelet count: %ld", candidate_lanelets.size()); std::vector goals; for (const auto & lane : candidate_lanelets) { @@ -282,14 +277,14 @@ std::vector MrmPullOverManager::find_goals_in_lanelets std::vector MrmPullOverManager::filter_nearby_goals( const std::vector & poses) { - RCLCPP_INFO(this->get_logger(), "pose count: %ld", poses.size()); + RCLCPP_DEBUG(this->get_logger(), "pose count: %ld", poses.size()); std::vector filtered_poses; auto it = poses.begin(); for (; it != poses.end(); ++it) { // filter unsafe yaw pose const double yaw_deviation = motion_utils::calcYawDeviation(trajectory_->points, *it); - RCLCPP_INFO(this->get_logger(), "yaw deviation to pose: %lf", yaw_deviation); + RCLCPP_DEBUG(this->get_logger(), "yaw deviation to pose: %lf", yaw_deviation); if (std::abs(yaw_deviation) > param_.yaw_deviation_threshold) { continue; } @@ -297,11 +292,12 @@ std::vector MrmPullOverManager::filter_nearby_goals( // filter too near pose // The poses are stored in order of distance, so the loop exits as soon as the first one // exceeding the threshold is found + // TODO(TomohitoAndo): Replace with better logic to filter the near goals const double arc_length_to_pose = motion_utils::calcSignedArcLength( trajectory_->points, odom_->pose.pose.position, it->position); const double distance_threshold = odom_->twist.twist.linear.x * param_.margin_time_to_goal; - RCLCPP_INFO(this->get_logger(), "distance to the pose: %lf", arc_length_to_pose); - RCLCPP_INFO(this->get_logger(), "distance threshold: %lf", distance_threshold); + RCLCPP_DEBUG(this->get_logger(), "distance to the pose: %lf", arc_length_to_pose); + RCLCPP_DEBUG(this->get_logger(), "distance threshold: %lf", distance_threshold); if (arc_length_to_pose > distance_threshold) { break; } From c9e9dffb9de4db099552462b0b0efdc7070f732d Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 16 Oct 2023 09:53:34 +0900 Subject: [PATCH 11/48] update pull over point Signed-off-by: Tomohito Ando --- system/mrm_pull_over_manager/config/pull_over_point.csv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_pull_over_manager/config/pull_over_point.csv b/system/mrm_pull_over_manager/config/pull_over_point.csv index 8e6a50d36f196..e8061f25967fa 100644 --- a/system/mrm_pull_over_manager/config/pull_over_point.csv +++ b/system/mrm_pull_over_manager/config/pull_over_point.csv @@ -1,5 +1,5 @@ x,y,z,yaw,lane_id 65401.7, 684.4, 715.8, 0, 19464 -65567.9, 681.5, 714.5, 0, 19779 +65626.6, 681.4, 714.0, 0, 19779 65808.1, 720.5, 712.6, -1.3, 20976 65570.5, 673.9, 714.5, 3.14, 20478 From 8411b80d4bb12a7d59465822494e6cd1989ce039 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 16 Oct 2023 16:44:26 +0900 Subject: [PATCH 12/48] chore(emergency_goal_manager): add main callbacks Signed-off-by: Makoto Kurihara --- .../emergency_goal_manager_node.hpp | 33 ++++++++-- system/emergency_goal_manager/package.xml | 3 + .../src/emergency_goal_manager_node.cpp | 66 ++++++++++++++++++- 3 files changed, 93 insertions(+), 9 deletions(-) diff --git a/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp b/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp index a763d2e1a5a23..18bd611f6dc60 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp @@ -16,22 +16,25 @@ #define EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ // Core -#include -#include +#include +#include +#include // Autoware +#include +#include +#include // ROS 2 core +#include #include +#include namespace emergency_goal_manager { struct Parameters { - int update_rate; // [Hz] - double target_acceleration; // [m/s^2] - double target_jerk; // [m/s^3] }; class EmergencyGoalManagerNode : public rclcpp::Node @@ -44,14 +47,32 @@ class EmergencyGoalManagerNode : public rclcpp::Node Parameters params_; // Subscriber + rclcpp::Subscription::SharedPtr sub_emergency_goals_; + rclcpp::Subscription::SharedPtr + sub_emergency_goals_clear_command_; + + void onEmergencyGoals(const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg); + void onEmergencyGoalsClearCommand( + const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg); // Server - + // Publisher + // Client + rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_; + rclcpp::Client::SharedPtr + client_set_mrm_route_points_; + rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_; + rclcpp::Client::SharedPtr client_clear_mrm_route_; + + void callSetMrmRoutePoints(); + void callClearMrmRoute(); + // Timer // States + std::unordered_map> emergency_goals_map_; // Algorithm }; diff --git a/system/emergency_goal_manager/package.xml b/system/emergency_goal_manager/package.xml index 150855de06b39..92d903ddd06ac 100644 --- a/system/emergency_goal_manager/package.xml +++ b/system/emergency_goal_manager/package.xml @@ -11,8 +11,11 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs + geometry_msgs rclcpp rclcpp_components + std_srvs tier4_system_msgs ament_lint_auto diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp index 00e017d0c4680..5b1fdb1b00160 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp @@ -14,6 +14,7 @@ #include + namespace emergency_goal_manager { @@ -21,21 +22,80 @@ EmergencyGoalManagerNode::EmergencyGoalManagerNode(const rclcpp::NodeOptions & n : Node("emergency_goal_manager_node", node_options) { // Parameter - params_.update_rate = static_cast(declare_parameter("update_rate", 30)); - params_.target_acceleration = declare_parameter("target_acceleration", -2.5); - params_.target_jerk = declare_parameter("target_jerk", -1.5); // Subscriber + sub_emergency_goals_ = create_subscription( + "~/input/emergency_goals", rclcpp::QoS{1}, + std::bind(&EmergencyGoalManagerNode::onEmergencyGoals, this, std::placeholders::_1)); + sub_emergency_goals_clear_command_ = + create_subscription( + "~/input/emergency_goals_clear_command", rclcpp::QoS{1}, + std::bind(&EmergencyGoalManagerNode::onEmergencyGoalsClearCommand, this, std::placeholders::_1)); // Server // Publisher + + // Client + client_set_mrm_route_points_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_set_mrm_route_points_ = create_client( + "~/output/set_mrm_route_points", rmw_qos_profile_services_default, + client_set_mrm_route_points_callback_group_); + client_clear_mrm_route_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_clear_mrm_route_ = create_client( + "~/output/clear_mrm_route_", rmw_qos_profile_services_default, + client_clear_mrm_route_callback_group_); // Timer // Initialize } +void EmergencyGoalManagerNode::onEmergencyGoals( + const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg) +{ + if (!emergency_goals_map_.empty()) { + emergency_goals_map_.clear(); + } + + std::queue emergency_goals_queue; + for (const auto & goal : msg->goals) { + emergency_goals_queue.push(goal); + } + emergency_goals_map_.emplace(msg->sender, emergency_goals_queue); + + callSetMrmRoutePoints(); +} + +void EmergencyGoalManagerNode::onEmergencyGoalsClearCommand( + const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg) +{ + if (emergency_goals_map_.count(msg->sender) == 0) { + RCLCPP_WARN(get_logger(), "Emergency goals from %s is empty.", msg->sender.c_str()); + } + + if (msg->command) { + emergency_goals_map_.erase(msg->sender); + + if (emergency_goals_map_.empty()) { + callClearMrmRoute(); + } else { + callSetMrmRoutePoints(); + } + } +} + +void EmergencyGoalManagerNode::callSetMrmRoutePoints() +{ + +} + +void EmergencyGoalManagerNode::callClearMrmRoute() +{ + auto request = std::make_shared(); +} } // namespace emergency_goal_manager From c779c5d22b34f5744a171e7804a2c51dfb8cc693 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 16 Oct 2023 20:52:17 +0900 Subject: [PATCH 13/48] replace activate pull over with operate mrm service Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.hpp | 10 +++++----- .../launch/mrm_pull_over_manager.launch.xml | 4 ++-- .../mrm_pull_over_manager_core.cpp | 19 +++++++++---------- 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 4d36a77fe98fd..019e686daa50c 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include // lanelet #include @@ -91,11 +91,11 @@ class MrmPullOverManager : public rclcpp::Node void on_trajectory(const Trajectory::ConstSharedPtr msg); // Server - rclcpp::Service::SharedPtr activate_pull_over_; + rclcpp::Service::SharedPtr operate_mrm_; - void activatePullOver( - const tier4_system_msgs::srv::ActivatePullOver::Request::SharedPtr request, - const tier4_system_msgs::srv::ActivatePullOver::Response::SharedPtr response); + void operateMrm( + const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, + const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response); // Publisher rclcpp::Publisher::SharedPtr pub_pose_array_; diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml index e7526153e1b76..f087f7c1acccc 100644 --- a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -3,7 +3,7 @@ - + @@ -16,7 +16,7 @@ - + i diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index e98c78089763f..b913468817f47 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -130,9 +130,8 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") create_publisher("~/output/mrm/pull_over/status", rclcpp::QoS{1}); // Server - activate_pull_over_ = create_service( - "~/input/mrm/pull_over/activate", - std::bind(&MrmPullOverManager::activatePullOver, this, _1, _2)); + operate_mrm_ = create_service( + "~/input/mrm/pull_over/operate", std::bind(&MrmPullOverManager::operateMrm, this, _1, _2)); // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -159,19 +158,19 @@ void MrmPullOverManager::publishStatus() const pub_status_->publish(status); } -void MrmPullOverManager::activatePullOver( - const tier4_system_msgs::srv::ActivatePullOver::Request::SharedPtr request, - const tier4_system_msgs::srv::ActivatePullOver::Response::SharedPtr response) +void MrmPullOverManager::operateMrm( + const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, + const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response) { - if (request->activate == true) { + if (request->operate == true) { const bool result = find_goals_within_route(); - response->status.success = result; + response->response.success = result; status_.state = MrmBehaviorStatus::OPERATING; } - if (request->activate == false) { + if (request->operate == false) { // TODO(TomohitoAndo): Call ClearEmergency service - response->status.success = true; + response->response.success = true; status_.state = MrmBehaviorStatus::AVAILABLE; } } From 16e9f0e569f24cf4eb87bc92b0f5f42b47117258 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 16 Oct 2023 21:10:50 +0900 Subject: [PATCH 14/48] publish emergency goals clear command when deactivated Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.hpp | 4 ++++ .../launch/mrm_pull_over_manager.launch.xml | 2 ++ .../mrm_pull_over_manager_core.cpp | 14 +++++++++++++- 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 019e686daa50c..4b3cff727a33b 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -33,6 +33,7 @@ #include #include +#include #include #include @@ -64,6 +65,7 @@ class MrmPullOverManager : public rclcpp::Node using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; using PoseLaneIdMap = std::map; using MrmBehaviorStatus = tier4_system_msgs::msg::MrmBehaviorStatus; + using EmergencyGoalsClearCommand = tier4_system_msgs::msg::EmergencyGoalsClearCommand; struct Parameter { @@ -100,6 +102,7 @@ class MrmPullOverManager : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_pose_array_; rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_emergency_goals_clear_command_; // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -115,6 +118,7 @@ class MrmPullOverManager : public rclcpp::Node // Algorithm bool is_data_ready(); void publishStatus() const; + void publishEmergencyGoalsClearComand() const; /** * @brief Find the goals within the lanelet and publish them diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml index f087f7c1acccc..bd848ebd1b340 100644 --- a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -6,6 +6,7 @@ + @@ -19,6 +20,7 @@ + i diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index b913468817f47..2a5bd6d118c2b 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -128,6 +128,8 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") create_publisher("~/output/mrm/pull_over/emergency_goals", rclcpp::QoS{1}); pub_status_ = create_publisher("~/output/mrm/pull_over/status", rclcpp::QoS{1}); + pub_emergency_goals_clear_command_ = create_publisher( + "~/output/mrm/pull_over/goals_clear_command", rclcpp::QoS{1}); // Server operate_mrm_ = create_service( @@ -158,6 +160,16 @@ void MrmPullOverManager::publishStatus() const pub_status_->publish(status); } +void MrmPullOverManager::publishEmergencyGoalsClearComand() const +{ + EmergencyGoalsClearCommand goals_clear_command; + goals_clear_command.stamp = this->now(); + goals_clear_command.command = true; + goals_clear_command.sender = "mrm_pull_over_manager"; + + pub_emergency_goals_clear_command_->publish(goals_clear_command); +} + void MrmPullOverManager::operateMrm( const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response) @@ -169,7 +181,7 @@ void MrmPullOverManager::operateMrm( } if (request->operate == false) { - // TODO(TomohitoAndo): Call ClearEmergency service + publishEmergencyGoalsClearComand(); response->response.success = true; status_.state = MrmBehaviorStatus::AVAILABLE; } From 0701a5353e072e26c44987541b5c74a709ecebc1 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 16 Oct 2023 22:13:18 +0900 Subject: [PATCH 15/48] replace PoseArray with EmergencyGoalsStamped Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.hpp | 10 +++--- .../mrm_pull_over_manager_core.cpp | 33 ++++++++++++------- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 4b3cff727a33b..9bfa41221f50a 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -24,7 +24,6 @@ // ROS 2 #include -#include #include // Autoware @@ -34,6 +33,7 @@ #include #include #include +#include #include #include @@ -58,7 +58,6 @@ class MrmPullOverManager : public rclcpp::Node private: using Pose = geometry_msgs::msg::Pose; - using PoseArray = geometry_msgs::msg::PoseArray; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using Odometry = nav_msgs::msg::Odometry; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; @@ -66,6 +65,7 @@ class MrmPullOverManager : public rclcpp::Node using PoseLaneIdMap = std::map; using MrmBehaviorStatus = tier4_system_msgs::msg::MrmBehaviorStatus; using EmergencyGoalsClearCommand = tier4_system_msgs::msg::EmergencyGoalsClearCommand; + using EmergencyGoalsStamped = tier4_system_msgs::msg::EmergencyGoalsStamped; struct Parameter { @@ -100,7 +100,7 @@ class MrmPullOverManager : public rclcpp::Node const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response); // Publisher - rclcpp::Publisher::SharedPtr pub_pose_array_; + rclcpp::Publisher::SharedPtr pub_emergency_goals_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_emergency_goals_clear_command_; @@ -118,7 +118,9 @@ class MrmPullOverManager : public rclcpp::Node // Algorithm bool is_data_ready(); void publishStatus() const; - void publishEmergencyGoalsClearComand() const; + void publishEmergencyGoalsClearCommand() const; + void publishEmergencyGoals(const std::vector & emergency_goals) const; + std::string get_module_name() const; /** * @brief Find the goals within the lanelet and publish them diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 2a5bd6d118c2b..64b6593a78fbe 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -124,8 +124,8 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") "~/input/trajectory", rclcpp::QoS{1}, std::bind(&MrmPullOverManager::on_trajectory, this, _1)); // Publisher - pub_pose_array_ = - create_publisher("~/output/mrm/pull_over/emergency_goals", rclcpp::QoS{1}); + pub_emergency_goals_ = create_publisher( + "~/output/mrm/pull_over/emergency_goals", rclcpp::QoS{1}); pub_status_ = create_publisher("~/output/mrm/pull_over/status", rclcpp::QoS{1}); pub_emergency_goals_clear_command_ = create_publisher( @@ -148,6 +148,11 @@ MrmPullOverManager::MrmPullOverManager() : Node("mrm_pull_over_manager") status_.state = MrmBehaviorStatus::AVAILABLE; } +std::string MrmPullOverManager::get_module_name() const +{ + return "mrm_pull_over_manager"; +} + void MrmPullOverManager::on_timer() { publishStatus(); @@ -160,12 +165,21 @@ void MrmPullOverManager::publishStatus() const pub_status_->publish(status); } -void MrmPullOverManager::publishEmergencyGoalsClearComand() const +void MrmPullOverManager::publishEmergencyGoals(const std::vector & emergency_goals) const +{ + EmergencyGoalsStamped emergency_goals_stamped; + emergency_goals_stamped.goals = emergency_goals; + emergency_goals_stamped.stamp = now(); + emergency_goals_stamped.sender = get_module_name(); + pub_emergency_goals_->publish(emergency_goals_stamped); +} + +void MrmPullOverManager::publishEmergencyGoalsClearCommand() const { EmergencyGoalsClearCommand goals_clear_command; goals_clear_command.stamp = this->now(); goals_clear_command.command = true; - goals_clear_command.sender = "mrm_pull_over_manager"; + goals_clear_command.sender = get_module_name(); pub_emergency_goals_clear_command_->publish(goals_clear_command); } @@ -181,7 +195,7 @@ void MrmPullOverManager::operateMrm( } if (request->operate == false) { - publishEmergencyGoalsClearComand(); + publishEmergencyGoalsClearCommand(); response->response.success = true; status_.state = MrmBehaviorStatus::AVAILABLE; } @@ -251,14 +265,11 @@ bool MrmPullOverManager::find_goals_within_route() const auto candidate_lanelets = lanelet_util::get_all_following_and_left_lanelets(route_handler_, *route_, current_lanelet); - PoseArray emergency_goals; - emergency_goals.header.frame_id = "map"; - emergency_goals.header.stamp = now(); - emergency_goals.poses = find_goals_in_lanelets(candidate_lanelets); + const auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); - emergency_goals.poses = filter_nearby_goals(emergency_goals.poses); + const auto filtered_emergency_goals = filter_nearby_goals(emergency_goals); - pub_pose_array_->publish(emergency_goals); + publishEmergencyGoals(filtered_emergency_goals); return true; } From 3d2e3cc305227c8e2cd37bb556430fafd77af9c4 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 16 Oct 2023 22:17:06 +0900 Subject: [PATCH 16/48] fix function name Signed-off-by: Tomohito Ando --- .../mrm_pull_over_manager_core.hpp | 6 +++--- .../mrm_pull_over_manager_core.cpp | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 9bfa41221f50a..52bd0b21ecca3 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -117,9 +117,9 @@ class MrmPullOverManager : public rclcpp::Node // Algorithm bool is_data_ready(); - void publishStatus() const; - void publishEmergencyGoalsClearCommand() const; - void publishEmergencyGoals(const std::vector & emergency_goals) const; + void publish_status() const; + void publish_emergency_goals_clear_command() const; + void publish_emergency_goals(const std::vector & emergency_goals) const; std::string get_module_name() const; /** diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 64b6593a78fbe..85ac0aa559d13 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -155,17 +155,17 @@ std::string MrmPullOverManager::get_module_name() const void MrmPullOverManager::on_timer() { - publishStatus(); + publish_status(); } -void MrmPullOverManager::publishStatus() const +void MrmPullOverManager::publish_status() const { auto status = status_; status.stamp = this->now(); pub_status_->publish(status); } -void MrmPullOverManager::publishEmergencyGoals(const std::vector & emergency_goals) const +void MrmPullOverManager::publish_emergency_goals(const std::vector & emergency_goals) const { EmergencyGoalsStamped emergency_goals_stamped; emergency_goals_stamped.goals = emergency_goals; @@ -174,7 +174,7 @@ void MrmPullOverManager::publishEmergencyGoals(const std::vector & emergen pub_emergency_goals_->publish(emergency_goals_stamped); } -void MrmPullOverManager::publishEmergencyGoalsClearCommand() const +void MrmPullOverManager::publish_emergency_goals_clear_command() const { EmergencyGoalsClearCommand goals_clear_command; goals_clear_command.stamp = this->now(); @@ -195,7 +195,7 @@ void MrmPullOverManager::operateMrm( } if (request->operate == false) { - publishEmergencyGoalsClearCommand(); + publish_emergency_goals_clear_command(); response->response.success = true; status_.state = MrmBehaviorStatus::AVAILABLE; } @@ -269,7 +269,7 @@ bool MrmPullOverManager::find_goals_within_route() const auto filtered_emergency_goals = filter_nearby_goals(emergency_goals); - publishEmergencyGoals(filtered_emergency_goals); + publish_emergency_goals(filtered_emergency_goals); return true; } From d620dfc68c78b92f112c031fefc3edaad01202ec Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 16 Oct 2023 22:58:37 +0900 Subject: [PATCH 17/48] fix condition of the num of goals Signed-off-by: Tomohito Ando --- .../src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 85ac0aa559d13..b519256b0eb77 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -288,7 +288,7 @@ std::vector MrmPullOverManager::find_goals_in_lanelets goals.emplace_back(it->second); } - if (goals.size() > param_.max_goal_pose_num) { + if (goals.size() >= param_.max_goal_pose_num) { break; } } From b2952c28a2a9ec182b8ba3bbe65635c70fcddfda Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 10 Nov 2023 20:44:36 +0900 Subject: [PATCH 18/48] create emergency_goal_manager node Signed-off-by: veqcc --- .../launch/system.launch.xml | 4 + system/emergency_goal_manager/CMakeLists.txt | 7 +- .../config/emergency_goal_manager.param.yaml | 6 +- ...de.hpp => emergency_goal_manager_core.hpp} | 38 ++---- .../launch/emergency_goal_manager.launch.xml | 80 ++--------- system/emergency_goal_manager/package.xml | 3 +- .../src/emergency_goal_manager_core.cpp | 124 ++++++++++++++++++ .../src/emergency_goal_manager_node.cpp | 99 ++------------ 8 files changed, 164 insertions(+), 197 deletions(-) rename system/emergency_goal_manager/include/{emergency_goal_manager/emergency_goal_manager_node.hpp => emergency_goal_manager_core.hpp} (85%) create mode 100644 system/emergency_goal_manager/src/emergency_goal_manager_core.cpp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 189e4edadb12a..88aea39e274d2 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -106,6 +106,10 @@ + + + + diff --git a/system/emergency_goal_manager/CMakeLists.txt b/system/emergency_goal_manager/CMakeLists.txt index 281cf2eeb53d5..834ed25931e14 100644 --- a/system/emergency_goal_manager/CMakeLists.txt +++ b/system/emergency_goal_manager/CMakeLists.txt @@ -4,14 +4,11 @@ project(emergency_goal_manager) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(emergency_goal_manager_component SHARED +ament_auto_add_executable(emergency_goal_manager src/emergency_goal_manager_node.cpp + src/emergency_goal_manager_core.cpp ) -rclcpp_components_register_node(emergency_goal_manager_component - PLUGIN "emergency_goal_manager::EmergencyGoalManager" - EXECUTABLE emergency_goal_manager) - ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml index 1ee2699a23a82..8b137891791fe 100644 --- a/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml +++ b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml @@ -1,5 +1 @@ -/**: - ros__parameters: - update_rate: 30 - target_acceleration: -2.5 - target_jerk: -1.5 + diff --git a/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp similarity index 85% rename from system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp rename to system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index 18bd611f6dc60..510c628dd3647 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager/emergency_goal_manager_node.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -15,11 +15,6 @@ #ifndef EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ #define EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ -// Core -#include -#include -#include - // Autoware #include #include @@ -30,21 +25,19 @@ #include #include -namespace emergency_goal_manager -{ +#include +#include +#include -struct Parameters +namespace emergency_goal_manager { -}; - -class EmergencyGoalManagerNode : public rclcpp::Node +class EmergencyGoalManager : public rclcpp::Node { public: - explicit EmergencyGoalManagerNode(const rclcpp::NodeOptions & node_options); + EmergencyGoalManager(); private: - // Parameters - Parameters params_; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; // Subscriber rclcpp::Subscription::SharedPtr sub_emergency_goals_; @@ -55,28 +48,19 @@ class EmergencyGoalManagerNode : public rclcpp::Node void onEmergencyGoalsClearCommand( const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg); - // Server - - // Publisher - // Client rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_; - rclcpp::Client::SharedPtr - client_set_mrm_route_points_; + rclcpp::Client::SharedPtr client_set_mrm_route_points_; rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_; rclcpp::Client::SharedPtr client_clear_mrm_route_; - - void callSetMrmRoutePoints(); - void callClearMrmRoute(); - - // Timer - // States + // Variables std::unordered_map> emergency_goals_map_; // Algorithm + void callSetMrmRoutePoints(); + void callClearMrmRoute(); }; - } // namespace emergency_goal_manager #endif // EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ diff --git a/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml index 769c121c18ac9..ef389207abcd1 100644 --- a/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml +++ b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml @@ -1,71 +1,9 @@ -# 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. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - config_file_path = LaunchConfiguration("config_file").perform(context) - with open(config_file_path, "r") as f: - params = yaml.safe_load(f)["/**"]["ros__parameters"] - - component = ComposableNode( - package="mrm_emergency_stop_operator", - plugin="mrm_emergency_stop_operator::MrmEmergencyStopOperator", - name="mrm_emergency_stop_operator", - parameters=[ - params, - ], - remappings=[ - ("~/input/mrm/emergency_stop/operate", "/system/mrm/emergency_stop/operate"), - ("~/input/control/control_cmd", "/control/command/control_cmd"), - ("~/output/mrm/emergency_stop/status", "/system/mrm/emergency_stop/status"), - ("~/output/mrm/emergency_stop/control_cmd", "/system/emergency/control_cmd"), - ], - ) - - container = ComposableNodeContainer( - name="mrm_emergency_stop_operator_container", - namespace="mrm_emergency_stop_operator", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - component, - ], - output="screen", - ) - - return [container] - - -def generate_launch_description(): - launch_arguments = [ - DeclareLaunchArgument( - "config_file", - default_value=[ - FindPackageShare("mrm_emergency_stop_operator"), - "/config/mrm_emergency_stop_operator.param.yaml", - ], - description="path to the parameter file of mrm_emergency_stop_operator", - ) - ] - - return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) + + + + + + + + + \ No newline at end of file diff --git a/system/emergency_goal_manager/package.xml b/system/emergency_goal_manager/package.xml index 92d903ddd06ac..5b7d6e35f0bc7 100644 --- a/system/emergency_goal_manager/package.xml +++ b/system/emergency_goal_manager/package.xml @@ -6,14 +6,13 @@ The emergency goal manager package Makoto Kurihara Tomohito Ando + Ryuta Kambe Apache License 2.0 ament_cmake_auto autoware_cmake autoware_adapi_v1_msgs - geometry_msgs - rclcpp rclcpp_components std_srvs tier4_system_msgs diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp new file mode 100644 index 0000000000000..9ba686eaadfa1 --- /dev/null +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -0,0 +1,124 @@ +// 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 + +namespace emergency_goal_manager +{ +EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") +{ + // Subscriber + sub_emergency_goals_ = create_subscription( + "~/input/emergency_goals", rclcpp::QoS{1}, + std::bind(&EmergencyGoalManager::onEmergencyGoals, this, std::placeholders::_1)); + sub_emergency_goals_clear_command_ = + create_subscription( + "~/input/emergency_goals_clear_command", rclcpp::QoS{1}, + std::bind(&EmergencyGoalManager::onEmergencyGoalsClearCommand, this, std::placeholders::_1)); + + // Client + client_set_mrm_route_points_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_set_mrm_route_points_ = create_client( + "/planning/mission_planning/mission_planner/srv/set_mrm_route", + rmw_qos_profile_services_default, + client_set_mrm_route_points_callback_group_); + client_clear_mrm_route_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_clear_mrm_route_ = create_client( + "/planning/mission_planning/mission_planner/srv/clear_mrm_route", + rmw_qos_profile_services_default, + client_clear_mrm_route_callback_group_); + + // Initialize + while (!client_set_mrm_route_points_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {} + while (!client_clear_mrm_route_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {} +} + +void EmergencyGoalManager::onEmergencyGoals( + const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg) +{ + if (!emergency_goals_map_.empty()) { + emergency_goals_map_.clear(); + } + + std::queue emergency_goals_queue; + for (const auto & goal : msg->goals) { + emergency_goals_queue.push(goal); + } + emergency_goals_map_.emplace(msg->sender, emergency_goals_queue); + + callSetMrmRoutePoints(); +} + +void EmergencyGoalManager::onEmergencyGoalsClearCommand( + const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg) +{ + if (emergency_goals_map_.count(msg->sender) == 0) { + RCLCPP_WARN(get_logger(), "Emergency goals from %s is empty.", msg->sender.c_str()); + } + + if (msg->command) { + emergency_goals_map_.erase(msg->sender); + + if (emergency_goals_map_.empty()) { + callClearMrmRoute(); + } else { + callSetMrmRoutePoints(); + } + } +} + +void EmergencyGoalManager::callSetMrmRoutePoints() +{ + auto request = std::make_shared(); + request->header.stamp = this->now(); + request->option.allow_goal_modification = true; + + while (!emergency_goals_map_.empty()) { + // TODO: set goals with the highest priority + auto goals = emergency_goals_map_.begin(); + + auto sender = goals->first; + auto goal_queue = goals->second; + if (goal_queue.empty()) { + emergency_goals_map_.erase(sender); + continue; + } + + request->goal = goal_queue.front(); + goal_queue.pop(); + + auto future = client_set_mrm_route_points_->async_send_request(request); + const auto duration = std::chrono::duration>(10); + if (future.wait_for(duration) != std::future_status::ready) { + continue; + } + if (future.get()->status.success == true) { + RCLCPP_INFO(get_logger(), "MRM route has been successfully sent."); + return; + } else { + continue; + } + } + + callClearMrmRoute(); +} + +void EmergencyGoalManager::callClearMrmRoute() +{ + auto request = std::make_shared(); + client_clear_mrm_route_->async_send_request(request); +} +} // namespace emergency_goal_manager \ No newline at end of file diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp index 5b1fdb1b00160..c307bbf70ef57 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,92 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "emergency_goal_manager_core.hpp" - -namespace emergency_goal_manager -{ - -EmergencyGoalManagerNode::EmergencyGoalManagerNode(const rclcpp::NodeOptions & node_options) -: Node("emergency_goal_manager_node", node_options) -{ - // Parameter - - // Subscriber - sub_emergency_goals_ = create_subscription( - "~/input/emergency_goals", rclcpp::QoS{1}, - std::bind(&EmergencyGoalManagerNode::onEmergencyGoals, this, std::placeholders::_1)); - sub_emergency_goals_clear_command_ = - create_subscription( - "~/input/emergency_goals_clear_command", rclcpp::QoS{1}, - std::bind(&EmergencyGoalManagerNode::onEmergencyGoalsClearCommand, this, std::placeholders::_1)); - - // Server - - // Publisher - - // Client - client_set_mrm_route_points_callback_group_ = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_set_mrm_route_points_ = create_client( - "~/output/set_mrm_route_points", rmw_qos_profile_services_default, - client_set_mrm_route_points_callback_group_); - client_clear_mrm_route_callback_group_ = - create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_clear_mrm_route_ = create_client( - "~/output/clear_mrm_route_", rmw_qos_profile_services_default, - client_clear_mrm_route_callback_group_); - - // Timer - - // Initialize -} - -void EmergencyGoalManagerNode::onEmergencyGoals( - const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg) +int main(int argc, char ** argv) { - if (!emergency_goals_map_.empty()) { - emergency_goals_map_.clear(); - } - - std::queue emergency_goals_queue; - for (const auto & goal : msg->goals) { - emergency_goals_queue.push(goal); - } - emergency_goals_map_.emplace(msg->sender, emergency_goals_queue); - - callSetMrmRoutePoints(); + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + + return 0; } - -void EmergencyGoalManagerNode::onEmergencyGoalsClearCommand( - const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg) -{ - if (emergency_goals_map_.count(msg->sender) == 0) { - RCLCPP_WARN(get_logger(), "Emergency goals from %s is empty.", msg->sender.c_str()); - } - - if (msg->command) { - emergency_goals_map_.erase(msg->sender); - - if (emergency_goals_map_.empty()) { - callClearMrmRoute(); - } else { - callSetMrmRoutePoints(); - } - } -} - -void EmergencyGoalManagerNode::callSetMrmRoutePoints() -{ - -} - -void EmergencyGoalManagerNode::callClearMrmRoute() -{ - auto request = std::make_shared(); -} - -} // namespace emergency_goal_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(emergency_goal_manager::EmergencyGoalManagerNode) From 113f07e8adc11e0fd20683c40b63f1fa9effac9a Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 10 Nov 2023 21:25:23 +0900 Subject: [PATCH 19/48] fix Signed-off-by: veqcc --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 1 + .../launch/mrm_pull_over_manager.launch.xml | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 9ba686eaadfa1..32dddbbc167ad 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -83,6 +83,7 @@ void EmergencyGoalManager::onEmergencyGoalsClearCommand( void EmergencyGoalManager::callSetMrmRoutePoints() { auto request = std::make_shared(); + request->header.frame_id = "map"; request->header.stamp = this->now(); request->option.allow_goal_modification = true; diff --git a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml index bd848ebd1b340..6828ca854a8c5 100644 --- a/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -21,7 +21,6 @@ -i From 5e7ef7c90887912876159f2e882f35b8cc4ba13c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 Nov 2023 23:59:01 +0000 Subject: [PATCH 20/48] style(pre-commit): autofix --- .../launch/system.launch.xml | 6 ++--- system/emergency_goal_manager/README.md | 24 +++++++++---------- .../config/emergency_goal_manager.param.yaml | 1 - .../include/emergency_goal_manager_core.hpp | 16 +++++++------ .../launch/emergency_goal_manager.launch.xml | 2 +- .../src/emergency_goal_manager_core.cpp | 24 +++++++++---------- system/mrm_pull_over_manager/README.md | 5 ---- .../mrm_pull_over_manager/csv_parser.hpp | 2 +- system/mrm_pull_over_manager/package.xml | 4 ++-- 9 files changed, 39 insertions(+), 45 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 88aea39e274d2..20471a7c9a2dd 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -103,12 +103,10 @@ - - + - - + diff --git a/system/emergency_goal_manager/README.md b/system/emergency_goal_manager/README.md index bf6a46ac8c841..c27ed818a50a4 100644 --- a/system/emergency_goal_manager/README.md +++ b/system/emergency_goal_manager/README.md @@ -10,29 +10,29 @@ The Emergency goal manager is responsible for coordinating the goal poses for em ### Input -| Name | Type | Description | -| ------------------------------------ | ---------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| | | | +| Name | Type | Description | +| ---- | ---- | ----------- | +| | | | ### Output -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ---------------------- | -| | | | +| Name | Type | Description | +| ---- | ---- | ----------- | +| | | | ## Parameters ### Node Parameters -| Name | Type | Default value | Explanation | -| ----------- | ---- | ------------- | ----------------------------- | -| | | | | +| Name | Type | Default value | Explanation | +| ---- | ---- | ------------- | ----------- | +| | | | | ### Core Parameters -| Name | Type | Default value | Explanation | -| ------------------- | ------ | ------------- | ---------------------------------------------- | -| | | | | +| Name | Type | Default value | Explanation | +| ---- | ---- | ------------- | ----------- | +| | | | | ## Assumptions / Known limits diff --git a/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml index 8b137891791fe..e69de29bb2d1d 100644 --- a/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml +++ b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml @@ -1 +0,0 @@ - diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index 510c628dd3647..5870969375ab8 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ -#define EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ +#ifndef EMERGENCY_GOAL_MANAGER_CORE_HPP_ +#define EMERGENCY_GOAL_MANAGER_CORE_HPP_ // Autoware #include @@ -21,13 +21,14 @@ #include // ROS 2 core -#include #include + +#include #include -#include #include #include +#include namespace emergency_goal_manager { @@ -40,10 +41,11 @@ class EmergencyGoalManager : public rclcpp::Node using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; // Subscriber - rclcpp::Subscription::SharedPtr sub_emergency_goals_; + rclcpp::Subscription::SharedPtr + sub_emergency_goals_; rclcpp::Subscription::SharedPtr sub_emergency_goals_clear_command_; - + void onEmergencyGoals(const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg); void onEmergencyGoalsClearCommand( const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg); @@ -63,4 +65,4 @@ class EmergencyGoalManager : public rclcpp::Node }; } // namespace emergency_goal_manager -#endif // EMERGENCY_GOAL_MANAGER__EMERGENCY_GOAL_MANAGER_CORE_HPP_ +#endif // EMERGENCY_GOAL_MANAGER_CORE_HPP_ diff --git a/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml index ef389207abcd1..1d85c1172d06b 100644 --- a/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml +++ b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml @@ -6,4 +6,4 @@ - \ No newline at end of file + diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 32dddbbc167ad..6843bb7324b5f 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -24,26 +24,26 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") std::bind(&EmergencyGoalManager::onEmergencyGoals, this, std::placeholders::_1)); sub_emergency_goals_clear_command_ = create_subscription( - "~/input/emergency_goals_clear_command", rclcpp::QoS{1}, - std::bind(&EmergencyGoalManager::onEmergencyGoalsClearCommand, this, std::placeholders::_1)); + "~/input/emergency_goals_clear_command", rclcpp::QoS{1}, + std::bind(&EmergencyGoalManager::onEmergencyGoalsClearCommand, this, std::placeholders::_1)); // Client client_set_mrm_route_points_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client_set_mrm_route_points_ = create_client( "/planning/mission_planning/mission_planner/srv/set_mrm_route", - rmw_qos_profile_services_default, - client_set_mrm_route_points_callback_group_); + rmw_qos_profile_services_default, client_set_mrm_route_points_callback_group_); client_clear_mrm_route_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client_clear_mrm_route_ = create_client( "/planning/mission_planning/mission_planner/srv/clear_mrm_route", - rmw_qos_profile_services_default, - client_clear_mrm_route_callback_group_); + rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_); // Initialize - while (!client_set_mrm_route_points_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {} - while (!client_clear_mrm_route_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {} + while (!client_set_mrm_route_points_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + } + while (!client_clear_mrm_route_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + } } void EmergencyGoalManager::onEmergencyGoals( @@ -52,13 +52,13 @@ void EmergencyGoalManager::onEmergencyGoals( if (!emergency_goals_map_.empty()) { emergency_goals_map_.clear(); } - + std::queue emergency_goals_queue; for (const auto & goal : msg->goals) { emergency_goals_queue.push(goal); } emergency_goals_map_.emplace(msg->sender, emergency_goals_queue); - + callSetMrmRoutePoints(); } @@ -71,7 +71,7 @@ void EmergencyGoalManager::onEmergencyGoalsClearCommand( if (msg->command) { emergency_goals_map_.erase(msg->sender); - + if (emergency_goals_map_.empty()) { callClearMrmRoute(); } else { @@ -122,4 +122,4 @@ void EmergencyGoalManager::callClearMrmRoute() auto request = std::make_shared(); client_clear_mrm_route_->async_send_request(request); } -} // namespace emergency_goal_manager \ No newline at end of file +} // namespace emergency_goal_manager diff --git a/system/mrm_pull_over_manager/README.md b/system/mrm_pull_over_manager/README.md index efc659e162e79..23a6ed29442dc 100644 --- a/system/mrm_pull_over_manager/README.md +++ b/system/mrm_pull_over_manager/README.md @@ -2,7 +2,6 @@ ## Purpose - ## Inner-workings / Algorithms ### State Transitions @@ -11,18 +10,14 @@ ### Input - ### Output - ## Parameters ### Node Parameters - ### Core Parameters - ## Assumptions / Known limits TBD. diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp index 7ee47ac036012..dc844d2dc8383 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp @@ -100,4 +100,4 @@ int main() } } // namespace mrm_pull_over_manager -#endif // MRM_PULL_OVER_MANAGER__CSV_PARSER_HPP_ \ No newline at end of file +#endif // MRM_PULL_OVER_MANAGER__CSV_PARSER_HPP_ diff --git a/system/mrm_pull_over_manager/package.xml b/system/mrm_pull_over_manager/package.xml index 0e3d0b7cd15cf..2509c2d90d48f 100644 --- a/system/mrm_pull_over_manager/package.xml +++ b/system/mrm_pull_over_manager/package.xml @@ -13,14 +13,14 @@ autoware_auto_system_msgs + lanelet2_extension motion_utils nav_msgs rclcpp + route_handler std_msgs std_srvs tier4_system_msgs - lanelet2_extension - route_handler ament_lint_auto autoware_lint_common From d65b05645c9a5d76a729cae29918f78a5e7bb7e5 Mon Sep 17 00:00:00 2001 From: veqcc Date: Wed, 22 Nov 2023 15:37:21 +0900 Subject: [PATCH 21/48] fix: set timeout for ClearMRMMRoute Signed-off-by: veqcc --- .../src/emergency_goal_manager_core.cpp | 32 +++++++++++++++---- 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 6843bb7324b5f..de483fd2cc840 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -104,13 +104,16 @@ void EmergencyGoalManager::callSetMrmRoutePoints() auto future = client_set_mrm_route_points_->async_send_request(request); const auto duration = std::chrono::duration>(10); if (future.wait_for(duration) != std::future_status::ready) { + RCLCPP_WARN(get_logger(), "MRM Route service timeout."); continue; - } - if (future.get()->status.success == true) { - RCLCPP_INFO(get_logger(), "MRM route has been successfully sent."); - return; } else { - continue; + if (future.get()->status.success) { + RCLCPP_INFO(get_logger(), "MRM Route has been successfully sent."); + return; + } else { + RCLCPP_WARN(get_logger(), "MRM Route service has failed."); + continue; + } } } @@ -120,6 +123,23 @@ void EmergencyGoalManager::callSetMrmRoutePoints() void EmergencyGoalManager::callClearMrmRoute() { auto request = std::make_shared(); - client_clear_mrm_route_->async_send_request(request); + const auto duration = std::chrono::duration>(10); + + while (true) { + auto future = client_clear_mrm_route_->async_send_request(request); + + if (future.wait_for(duration) != std::future_status::ready) { + RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout."); + return; + } else { + if (future.get()->success) { + RCLCPP_INFO(get_logger(), "Clear MRM Route has been successfully sent."); + return; + } else { + RCLCPP_WARN(get_logger(), "Clear MRM Route has failed."); + continue; + } + } + } } } // namespace emergency_goal_manager From 8b59d1618d82da5b70368bab76a980e57725d8d8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Dec 2023 14:25:09 +0900 Subject: [PATCH 22/48] fix optional to build Signed-off-by: kosuke55 --- .../src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index b519256b0eb77..09c5f9cbbe196 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -46,11 +46,11 @@ lanelet::ConstLanelets get_all_left_lanelets( lanelet::ConstLanelets left_lanalets; auto left_lanelet = route_handler.getLeftLanelet(base_lanelet, false, true); while (left_lanelet) { - left_lanalets.emplace_back(left_lanelet.get()); + left_lanalets.emplace_back(left_lanelet.value()); RCLCPP_DEBUG(rclcpp::get_logger(__func__), "left lanelet id: %ld", left_lanelet->id()); // get next left lanelet - left_lanelet = route_handler.getLeftLanelet(left_lanelet.get(), false, true); + left_lanelet = route_handler.getLeftLanelet(left_lanelet.value(), false, true); } return left_lanalets; From 10dbca23cdc187053ba51f8551f6182c16bba6a2 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Dec 2023 22:11:15 +0900 Subject: [PATCH 23/48] feat(mission_planner): use ClearRoute instead of Trigger Signed-off-by: kosuke55 --- .../include/mission_planner/mission_planner_interface.hpp | 2 +- .../src/mission_planner/mission_planner.cpp | 8 ++++---- .../include/emergency_goal_manager_core.hpp | 4 +++- .../src/emergency_goal_manager_core.cpp | 6 +++--- 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp index 916dabfdbba2b..ddd0bfa652f0e 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -32,7 +32,7 @@ struct SetMrmRoute struct ClearMrmRoute { - using Service = std_srvs::srv::Trigger; + using Service = autoware_adapi_v1_msgs::srv::ClearRoute; static constexpr char name[] = "~/srv/clear_mrm_route"; }; diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 513a5b4c25a2e..4940faa6f81ee 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -497,7 +497,7 @@ void MissionPlanner::on_clear_mrm_route( if (!normal_route_) { clear_mrm_route(); change_state(RouteState::Message::UNSET); - res->success = true; + res->status.success = true; return; } @@ -506,7 +506,7 @@ void MissionPlanner::on_clear_mrm_route( clear_mrm_route(); change_route(*normal_route_); change_state(RouteState::Message::SET); - res->success = true; + res->status.success = true; return; } @@ -523,12 +523,12 @@ void MissionPlanner::on_clear_mrm_route( change_mrm_route(*mrm_route_); change_route(*normal_route_); change_state(RouteState::Message::SET); - res->success = false; + res->status.success = false; } else { clear_mrm_route(); change_route(new_route); change_state(RouteState::Message::SET); - res->success = true; + res->status.success = true; } } diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index 5870969375ab8..6d6db5ea9ca59 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -16,6 +16,7 @@ #define EMERGENCY_GOAL_MANAGER_CORE_HPP_ // Autoware +#include #include #include #include @@ -39,6 +40,7 @@ class EmergencyGoalManager : public rclcpp::Node private: using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; // Subscriber rclcpp::Subscription::SharedPtr @@ -54,7 +56,7 @@ class EmergencyGoalManager : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_; rclcpp::Client::SharedPtr client_set_mrm_route_points_; rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_; - rclcpp::Client::SharedPtr client_clear_mrm_route_; + rclcpp::Client::SharedPtr client_clear_mrm_route_; // Variables std::unordered_map> emergency_goals_map_; diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index de483fd2cc840..f05e049abe3f4 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -35,7 +35,7 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") rmw_qos_profile_services_default, client_set_mrm_route_points_callback_group_); client_clear_mrm_route_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_clear_mrm_route_ = create_client( + client_clear_mrm_route_ = create_client( "/planning/mission_planning/mission_planner/srv/clear_mrm_route", rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_); @@ -122,7 +122,7 @@ void EmergencyGoalManager::callSetMrmRoutePoints() void EmergencyGoalManager::callClearMrmRoute() { - auto request = std::make_shared(); + auto request = std::make_shared(); const auto duration = std::chrono::duration>(10); while (true) { @@ -132,7 +132,7 @@ void EmergencyGoalManager::callClearMrmRoute() RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout."); return; } else { - if (future.get()->success) { + if (future.get()->status.success) { RCLCPP_INFO(get_logger(), "Clear MRM Route has been successfully sent."); return; } else { From 68bf448ea851d917d9e351b5ed53508baae2e9b0 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 15:27:40 +0900 Subject: [PATCH 24/48] fix reference goal queue Signed-off-by: kosuke55 --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index f05e049abe3f4..1b623322fd21c 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -92,7 +92,7 @@ void EmergencyGoalManager::callSetMrmRoutePoints() auto goals = emergency_goals_map_.begin(); auto sender = goals->first; - auto goal_queue = goals->second; + auto & goal_queue = goals->second; if (goal_queue.empty()) { emergency_goals_map_.erase(sender); continue; From b5d3b03cd7dc132282a9355abe26ad93a6ac4474 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 15:27:55 +0900 Subject: [PATCH 25/48] while (rclcpp::ok()) Signed-off-by: kosuke55 --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 1b623322fd21c..e2b82990f842f 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -125,7 +125,7 @@ void EmergencyGoalManager::callClearMrmRoute() auto request = std::make_shared(); const auto duration = std::chrono::duration>(10); - while (true) { + while (rclcpp::ok()) { auto future = client_clear_mrm_route_->async_send_request(request); if (future.wait_for(duration) != std::future_status::ready) { From 114cc76bb1c663ca9d20815e604d77c5f38c0d76 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 21:55:40 +0900 Subject: [PATCH 26/48] add timeout Signed-off-by: kosuke55 --- .../src/emergency_goal_manager_core.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index e2b82990f842f..c2c907c38cc27 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -124,10 +124,15 @@ void EmergencyGoalManager::callClearMrmRoute() { auto request = std::make_shared(); const auto duration = std::chrono::duration>(10); + const auto start_time = std::chrono::steady_clock::now(); while (rclcpp::ok()) { - auto future = client_clear_mrm_route_->async_send_request(request); + if (std::chrono::steady_clock::now() - start_time > duration) { + RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout."); + return; + } + auto future = client_clear_mrm_route_->async_send_request(request); if (future.wait_for(duration) != std::future_status::ready) { RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout."); return; From 197cc944543f8faa9dc997227c73c28807499e7b Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 21:56:03 +0900 Subject: [PATCH 27/48] feat(mission_planner): print modified goal reroute Signed-off-by: kosuke55 --- .../mission_planner/src/mission_planner/mission_planner.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 3b5e3217f5beb..2890015ad4788 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -444,6 +444,7 @@ void MissionPlanner::on_set_mrm_route( res->status.success = false; } change_state(RouteState::Message::SET); + RCLCPP_INFO(get_logger(), "Route is successfully changed with the modified goal"); return; } @@ -452,6 +453,7 @@ void MissionPlanner::on_set_mrm_route( change_mrm_route(new_route); change_state(RouteState::Message::SET); res->status.success = true; + RCLCPP_INFO(get_logger(), "MRM route is successfully changed with the modified goal"); return; } @@ -537,6 +539,8 @@ void MissionPlanner::on_clear_mrm_route( void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) { + RCLCPP_INFO(get_logger(), "Received modified goal."); + if (state_.state != RouteState::Message::SET) { RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); return; @@ -572,6 +576,7 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt change_mrm_route(new_route); change_state(RouteState::Message::SET); + RCLCPP_INFO(get_logger(), "Changed the MRM route with the modified goal"); return; } @@ -593,6 +598,7 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt change_route(new_route); change_state(RouteState::Message::SET); + RCLCPP_INFO(get_logger(), "Changed the route with the modified goal"); return; } From 756c5946711d654d2971d483a2517072e57739b2 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 21:45:58 +0900 Subject: [PATCH 28/48] add sleep after service fails Signed-off-by: kosuke55 --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index c2c907c38cc27..b5eed14ab89a2 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -112,6 +112,7 @@ void EmergencyGoalManager::callSetMrmRoutePoints() return; } else { RCLCPP_WARN(get_logger(), "MRM Route service has failed."); + std::this_thread::sleep_for(std::chrono::seconds(1)); continue; } } @@ -143,6 +144,7 @@ void EmergencyGoalManager::callClearMrmRoute() } else { RCLCPP_WARN(get_logger(), "Clear MRM Route has failed."); continue; + std::this_thread::sleep_for(std::chrono::seconds(1)); } } } From 5dcec441841c5944af95552a2945ef68a72f70f0 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 21:45:27 +0900 Subject: [PATCH 29/48] fix(behavior_path_planner): reset previou modified goal Signed-off-by: kosuke55 --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 c006e6d1470b6..f47b3241eafcf 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -511,7 +511,9 @@ void BehaviorPathPlannerNode::run() // Reset behavior tree when new route is received, // so that the each modules do not have to care about the "route jump". if (!is_first_time && !has_same_route_id) { + RCLCPP_INFO(get_logger(), "New uuid route is received. Resetting modules."); planner_manager_->reset(); + planner_data_->prev_modified_goal.reset(); } } From eaf26fd131a0566db0809d29f64e6bcb0920bb08 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 22:40:03 +0900 Subject: [PATCH 30/48] fix sleep Signed-off-by: kosuke55 --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index b5eed14ab89a2..30e09ce848085 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -142,9 +142,9 @@ void EmergencyGoalManager::callClearMrmRoute() RCLCPP_INFO(get_logger(), "Clear MRM Route has been successfully sent."); return; } else { + std::this_thread::sleep_for(std::chrono::seconds(1)); RCLCPP_WARN(get_logger(), "Clear MRM Route has failed."); continue; - std::this_thread::sleep_for(std::chrono::seconds(1)); } } } From 37183b7a449b780c033d0602b76356a232d810be Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 23:39:38 +0900 Subject: [PATCH 31/48] feat(behavior_path_planner): allow reroute when always executable module running or candidate modules Signed-off-by: kosuke55 --- .../behavior_path_planner_node.hpp | 2 +- .../behavior_path_planner/planner_manager.hpp | 7 +++++++ .../src/behavior_path_planner_node.cpp | 14 +++++--------- .../behavior_path_planner/src/planner_manager.cpp | 5 +---- 4 files changed, 14 insertions(+), 14 deletions(-) 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 39b470d0df449..207b0faba63ab 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 @@ -173,7 +173,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish reroute availability */ - void publish_reroute_availability(); + void publish_reroute_availability() const; /** * @brief publish steering factor from intersection 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 8bd5195cfbcd9..d2cfd0d1a98a5 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 @@ -222,6 +222,13 @@ class PlannerManager */ bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } + bool hasNonAlwaysExecutableApprovedModules() const + { + return std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + } + /** * @brief check if there are candidate modules. */ 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 f47b3241eafcf..341421d87844c 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -654,18 +654,14 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } -void BehaviorPathPlannerNode::publish_reroute_availability() +void BehaviorPathPlannerNode::publish_reroute_availability() const { - const bool has_approved_modules = planner_manager_->hasApprovedModules(); - const bool has_candidate_modules = planner_manager_->hasCandidateModules(); - - // In the current behavior path planner, we might get unexpected behavior when rerouting while - // modules other than lane follow are active. Therefore, rerouting will be allowed only when the - // lane follow module is running Note that if there is a approved module or candidate module, it - // means non-lane-following modules are runnning. + // In the current behavior path planner, we might encounter unexpected behavior when rerouting + // while modules other than lane following are active. If non-lane-following module except + // always-executable module is approved and running, rerouting will not be　possible. RerouteAvailability is_reroute_available; is_reroute_available.stamp = this->now(); - if (has_approved_modules || has_candidate_modules) { + if (planner_manager_->hasNonAlwaysExecutableApprovedModules()) { is_reroute_available.availability = false; } else { is_reroute_available.availability = true; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index e97f04e83eea8..2c51ab1f85cf7 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -222,10 +222,7 @@ std::vector PlannerManager::getRequestModules( // Condition 1: always executable module can be added regardless of the existence of other // modules, so skip checking the existence of other modules. // in other cases, need to check the existence of other modules and which module can be added. - const bool has_non_always_executable_module = std::any_of( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); - if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + if (!manager_ptr->isAlwaysExecutableModule() && hasNonAlwaysExecutableApprovedModules()) { // pairs of find_block_module and is_executable std::vector, std::function>> conditions; From a062b7a4764b5f5dce35218d7655ced8efe8c27f Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 23:40:37 +0900 Subject: [PATCH 32/48] feat(mission_planner): judge reroute safe when ego is stopped Signed-off-by: kosuke55 --- .../src/mission_planner/mission_planner.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 2890015ad4788..d44f5700e1137 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -735,6 +735,13 @@ bool MissionPlanner::check_reroute_safety( return false; } + const auto current_velocity = odometry_->twist.twist.linear.x; + + // if vehicle is stopped, do not check safety + if (current_velocity < 0.01) { + return true; + } + auto hasSamePrimitives = []( const std::vector & original_primitives, const std::vector & target_primitives) { @@ -879,13 +886,17 @@ bool MissionPlanner::check_reroute_safety( } // check safety - const auto current_velocity = odometry_->twist.twist.linear.x; const double safety_length = std::max(current_velocity * reroute_time_threshold_, minimum_reroute_length_); if (accumulated_length > safety_length) { return true; } + RCLCPP_WARN( + get_logger(), + "Length of lane where original and B target (= %f) is less than safety length (= %f), so " + "reroute is not safe.", + accumulated_length, safety_length); return false; } } // namespace mission_planner From 83e60b87fa68d60c7d50fcadaea29ed0ca52ae5b Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 23:41:31 +0900 Subject: [PATCH 33/48] fix(mission_planner): fix reroute chattering when mrm route clear fails Signed-off-by: kosuke55 --- .../mission_planner/src/mission_planner/mission_planner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index d44f5700e1137..a1536a1bebfbf 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -524,9 +524,8 @@ void MissionPlanner::on_clear_mrm_route( // check new route safety if (new_route.segments.empty() || !check_reroute_safety(*mrm_route_, new_route)) { // failed to create a new route - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "Reroute with normal goal failed."); + RCLCPP_ERROR(get_logger(), "Reroute with normal goal failed."); change_mrm_route(*mrm_route_); - change_route(*normal_route_); change_state(RouteState::Message::SET); res->status.success = false; } else { From c4ed177b0925d475fe32c62f89a510936971fc76 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 5 Dec 2023 23:54:18 +0900 Subject: [PATCH 34/48] modify error message Signed-off-by: kosuke55 --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 30e09ce848085..07cf8d895c454 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -129,7 +129,7 @@ void EmergencyGoalManager::callClearMrmRoute() while (rclcpp::ok()) { if (std::chrono::steady_clock::now() - start_time > duration) { - RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout."); + RCLCPP_WARN(get_logger(), "Clear MRM Route operation timeout."); return; } From f98a3db822d0554324e20d305939fee051d13231 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 5 Mar 2024 17:11:49 +0900 Subject: [PATCH 35/48] chore(emergency_goal_manager): fix todo Signed-off-by: Makoto Kurihara --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 07cf8d895c454..58a92e4eabc26 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -88,7 +88,7 @@ void EmergencyGoalManager::callSetMrmRoutePoints() request->option.allow_goal_modification = true; while (!emergency_goals_map_.empty()) { - // TODO: set goals with the highest priority + // TODO(Makoto Kurihara): set goals with the highest priority auto goals = emergency_goals_map_.begin(); auto sender = goals->first; From 8a4cd758c55b10370240b1f0bb9754255aac6e8c Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 11 Mar 2024 19:18:32 +0900 Subject: [PATCH 36/48] fix(mrm_pull_over_manager): fix typo Signed-off-by: Makoto Kurihara --- .../mrm_pull_over_manager/mrm_pull_over_manager_core.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 09c5f9cbbe196..1343ed6efd6cd 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -43,17 +43,17 @@ lanelet::ConstLanelet get_current_lanelet( lanelet::ConstLanelets get_all_left_lanelets( const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & base_lanelet) { - lanelet::ConstLanelets left_lanalets; + lanelet::ConstLanelets left_lanelets; auto left_lanelet = route_handler.getLeftLanelet(base_lanelet, false, true); while (left_lanelet) { - left_lanalets.emplace_back(left_lanelet.value()); + left_lanelets.emplace_back(left_lanelet.value()); RCLCPP_DEBUG(rclcpp::get_logger(__func__), "left lanelet id: %ld", left_lanelet->id()); // get next left lanelet left_lanelet = route_handler.getLeftLanelet(left_lanelet.value(), false, true); } - return left_lanalets; + return left_lanelets; } /** From bceda6b56a55992fd589716e388c1fb588b91d12 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 11 Mar 2024 19:19:14 +0900 Subject: [PATCH 37/48] fix(mrm_pull_over_manager): fix typo Signed-off-by: Makoto Kurihara --- .../mrm_pull_over_manager/mrm_pull_over_manager_core.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp index 52bd0b21ecca3..7eca5ec0411cc 100644 --- a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -76,7 +76,7 @@ class MrmPullOverManager : public rclcpp::Node double margin_time_to_goal; }; - // Subscribtoers + // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_map_; From ba1b293c202479295e55a8a9c9d7e7d1a153fe35 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 22 Mar 2024 14:14:13 +0900 Subject: [PATCH 38/48] chore(mrm_pull_over_manager): remove the command field from the clear msg Signed-off-by: Makoto Kurihara --- .../src/emergency_goal_manager_core.cpp | 12 +++++------- .../mrm_pull_over_manager_core.cpp | 1 - 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 58a92e4eabc26..08df9f460017c 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -69,14 +69,12 @@ void EmergencyGoalManager::onEmergencyGoalsClearCommand( RCLCPP_WARN(get_logger(), "Emergency goals from %s is empty.", msg->sender.c_str()); } - if (msg->command) { - emergency_goals_map_.erase(msg->sender); + emergency_goals_map_.erase(msg->sender); - if (emergency_goals_map_.empty()) { - callClearMrmRoute(); - } else { - callSetMrmRoutePoints(); - } + if (emergency_goals_map_.empty()) { + callClearMrmRoute(); + } else { + callSetMrmRoutePoints(); } } diff --git a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp index 1343ed6efd6cd..8974f5de691f4 100644 --- a/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -178,7 +178,6 @@ void MrmPullOverManager::publish_emergency_goals_clear_command() const { EmergencyGoalsClearCommand goals_clear_command; goals_clear_command.stamp = this->now(); - goals_clear_command.command = true; goals_clear_command.sender = get_module_name(); pub_emergency_goals_clear_command_->publish(goals_clear_command); From 341730be0737ff57909c4627025068296849641b Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 22 Mar 2024 15:44:54 +0900 Subject: [PATCH 39/48] docs(emergency_goal_manager): add readme Signed-off-by: Makoto Kurihara --- system/emergency_goal_manager/README.md | 28 ++++++++++--------------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/system/emergency_goal_manager/README.md b/system/emergency_goal_manager/README.md index c27ed818a50a4..3ecc18db82c32 100644 --- a/system/emergency_goal_manager/README.md +++ b/system/emergency_goal_manager/README.md @@ -6,33 +6,27 @@ The Emergency goal manager is responsible for coordinating the goal poses for em ## Inner-workings / Algorithms +TBD. + ## Inputs / Outputs ### Input -| Name | Type | Description | -| ---- | ---- | ----------- | -| | | | +| Name | Type | Description | +| --------------------------------------- | ---------------------------------------------------- | ----------------------------- | +| `~/input/emergency_goals` | `tier4_system_msgs::msg::EmergencyGoalsStamped` | Candidates for emergency goal | +| `~/input/emergency_goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command | ### Output -| Name | Type | Description | -| ---- | ---- | ----------- | -| | | | +| Name | Type | Description | +| ---------------------------------------------------------------- | --------------------------------------------- | ------------------------ | +| `/planning/mission_planning/mission_planner/srv/set_mrm_route` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | Set route points for MRM | +| `/planning/mission_planning/mission_planner/srv/clear_mrm_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | Clear route for MRM | ## Parameters -### Node Parameters - -| Name | Type | Default value | Explanation | -| ---- | ---- | ------------- | ----------- | -| | | | | - -### Core Parameters - -| Name | Type | Default value | Explanation | -| ---- | ---- | ------------- | ----------- | -| | | | | +No parameters. ## Assumptions / Known limits From 92933eed0f2aa8c06700feabf7b310c9b2f8fb24 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 22 Mar 2024 17:43:55 +0900 Subject: [PATCH 40/48] docs(mrm_pull_over_manager): add readme Signed-off-by: Makoto Kurihara --- system/mrm_pull_over_manager/README.md | 30 ++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/system/mrm_pull_over_manager/README.md b/system/mrm_pull_over_manager/README.md index 23a6ed29442dc..74925c4755f02 100644 --- a/system/mrm_pull_over_manager/README.md +++ b/system/mrm_pull_over_manager/README.md @@ -2,22 +2,52 @@ ## Purpose +This node sends candidate pull over poses to emergency_goal_manager according to the pull over MRM request. + ## Inner-workings / Algorithms +TBD. + ### State Transitions +TBD. + ## Inputs / Outputs ### Input +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | -------------------------------- | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | To get current pose and velocity | +| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | To get current route | +| `~/input/lanelet_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | To calculate pull over points | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | To calculate pull over points | +| `~/input/mrm/pull_over/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM pull over request | + ### Output +| Name | Type | Description | +| -------------------------------------------- | ---------------------------------------------------- | ------------------------- | +| `~/output/mrm/pull_over/emergency_goals` | `tier4_system_msgs::msg::EmergencyGoalsStamped` | Candidate pull over poses | +| `~/output/mrm/pull_over/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Pull over MRM status | +| `~/output/mrm/pull_over/goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command | + ## Parameters ### Node Parameters +| Name | Type | Description | Default | +| ------------------------- | ------ | ------------------------------------------------------ | ------- | +| `update_rate` | double | Timer callback update rate [Hz] | 10.0 | + ### Core Parameters +| Name | Type | Description | Default | +| ------------------------- | ------ | ------------------------------------------------------ | ------- | +| `max_goal_pose_num` | int | Maximum number of candidate goal poses [-] | 3 | +| `yaw_deviation_threshold` | double | Yaw deviation threshold for candidate goal poses [rad] | 0.5 | +| `margin_time_to_goal` | double | Arrival time threshold for candidate goal poses [sec] | 10.0 | + ## Assumptions / Known limits TBD. From 908aa4a5c483a3a776fb8b9b4fa5623ec4e7d087 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 22 Mar 2024 08:46:39 +0000 Subject: [PATCH 41/48] style(pre-commit): autofix --- system/emergency_goal_manager/README.md | 16 +++++----- system/mrm_pull_over_manager/README.md | 40 ++++++++++++------------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/system/emergency_goal_manager/README.md b/system/emergency_goal_manager/README.md index 3ecc18db82c32..83e2342cba3fc 100644 --- a/system/emergency_goal_manager/README.md +++ b/system/emergency_goal_manager/README.md @@ -12,17 +12,17 @@ TBD. ### Input -| Name | Type | Description | -| --------------------------------------- | ---------------------------------------------------- | ----------------------------- | -| `~/input/emergency_goals` | `tier4_system_msgs::msg::EmergencyGoalsStamped` | Candidates for emergency goal | -| `~/input/emergency_goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command | +| Name | Type | Description | +| --------------------------------------- | ---------------------------------------------------- | ----------------------------- | +| `~/input/emergency_goals` | `tier4_system_msgs::msg::EmergencyGoalsStamped` | Candidates for emergency goal | +| `~/input/emergency_goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command | ### Output -| Name | Type | Description | -| ---------------------------------------------------------------- | --------------------------------------------- | ------------------------ | -| `/planning/mission_planning/mission_planner/srv/set_mrm_route` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | Set route points for MRM | -| `/planning/mission_planning/mission_planner/srv/clear_mrm_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | Clear route for MRM | +| Name | Type | Description | +| ---------------------------------------------------------------- | --------------------------------------------- | ------------------------ | +| `/planning/mission_planning/mission_planner/srv/set_mrm_route` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | Set route points for MRM | +| `/planning/mission_planning/mission_planner/srv/clear_mrm_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | Clear route for MRM | ## Parameters diff --git a/system/mrm_pull_over_manager/README.md b/system/mrm_pull_over_manager/README.md index 74925c4755f02..a637ef8a43e22 100644 --- a/system/mrm_pull_over_manager/README.md +++ b/system/mrm_pull_over_manager/README.md @@ -16,37 +16,37 @@ TBD. ### Input -| Name | Type | Description | -| ------------------------------- | ---------------------------------------------- | -------------------------------- | -| `~/input/odometry` | `nav_msgs::msg::Odometry` | To get current pose and velocity | -| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | To get current route | -| `~/input/lanelet_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | To calculate pull over points | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | To calculate pull over points | -| `~/input/mrm/pull_over/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM pull over request | +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | -------------------------------- | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | To get current pose and velocity | +| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | To get current route | +| `~/input/lanelet_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | To calculate pull over points | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | To calculate pull over points | +| `~/input/mrm/pull_over/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM pull over request | ### Output -| Name | Type | Description | -| -------------------------------------------- | ---------------------------------------------------- | ------------------------- | -| `~/output/mrm/pull_over/emergency_goals` | `tier4_system_msgs::msg::EmergencyGoalsStamped` | Candidate pull over poses | -| `~/output/mrm/pull_over/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Pull over MRM status | -| `~/output/mrm/pull_over/goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command | +| Name | Type | Description | +| -------------------------------------------- | ---------------------------------------------------- | ------------------------- | +| `~/output/mrm/pull_over/emergency_goals` | `tier4_system_msgs::msg::EmergencyGoalsStamped` | Candidate pull over poses | +| `~/output/mrm/pull_over/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Pull over MRM status | +| `~/output/mrm/pull_over/goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command | ## Parameters ### Node Parameters -| Name | Type | Description | Default | -| ------------------------- | ------ | ------------------------------------------------------ | ------- | -| `update_rate` | double | Timer callback update rate [Hz] | 10.0 | +| Name | Type | Description | Default | +| ------------- | ------ | ------------------------------- | ------- | +| `update_rate` | double | Timer callback update rate [Hz] | 10.0 | ### Core Parameters -| Name | Type | Description | Default | -| ------------------------- | ------ | ------------------------------------------------------ | ------- | -| `max_goal_pose_num` | int | Maximum number of candidate goal poses [-] | 3 | -| `yaw_deviation_threshold` | double | Yaw deviation threshold for candidate goal poses [rad] | 0.5 | -| `margin_time_to_goal` | double | Arrival time threshold for candidate goal poses [sec] | 10.0 | +| Name | Type | Description | Default | +| ------------------------- | ------ | ------------------------------------------------------ | ------- | +| `max_goal_pose_num` | int | Maximum number of candidate goal poses [-] | 3 | +| `yaw_deviation_threshold` | double | Yaw deviation threshold for candidate goal poses [rad] | 0.5 | +| `margin_time_to_goal` | double | Arrival time threshold for candidate goal poses [sec] | 10.0 | ## Assumptions / Known limits From 10723f4cd1ae6fe7257a626e10ff991232327ac0 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 2 Apr 2024 18:13:21 +0900 Subject: [PATCH 42/48] chore(emergency_goal_manager): fix copyright year Signed-off-by: Makoto Kurihara --- .../include/emergency_goal_manager_core.hpp | 2 +- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index 6d6db5ea9ca59..46c3bcd15b442 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 08df9f460017c..e6367717cfec6 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From ee8146642a3865bb2c5550060cd41a8bff43f262 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 2 Apr 2024 18:13:55 +0900 Subject: [PATCH 43/48] chore(emergency_goal_manager): remove comments in the include section Signed-off-by: Makoto Kurihara --- .../include/emergency_goal_manager_core.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index 46c3bcd15b442..faa849e1338cd 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -15,13 +15,11 @@ #ifndef EMERGENCY_GOAL_MANAGER_CORE_HPP_ #define EMERGENCY_GOAL_MANAGER_CORE_HPP_ -// Autoware #include #include #include #include -// ROS 2 core #include #include From abfe52ebf02bc6d5ff160f4e3099b0139602a0f0 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 2 Apr 2024 18:14:24 +0900 Subject: [PATCH 44/48] fix(emergency_goal_manager): remove the clear process Signed-off-by: Makoto Kurihara --- .../emergency_goal_manager/src/emergency_goal_manager_core.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index e6367717cfec6..a45782f689e0c 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -49,9 +49,6 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") void EmergencyGoalManager::onEmergencyGoals( const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg) { - if (!emergency_goals_map_.empty()) { - emergency_goals_map_.clear(); - } std::queue emergency_goals_queue; for (const auto & goal : msg->goals) { From d4364ef1cb1db3ec103cf04db8124eaf961150aa Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 2 Apr 2024 09:17:38 +0000 Subject: [PATCH 45/48] style(pre-commit): autofix --- .../include/emergency_goal_manager_core.hpp | 9 ++++----- .../src/emergency_goal_manager_core.cpp | 1 - 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index faa849e1338cd..9f947816cd542 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -15,15 +15,14 @@ #ifndef EMERGENCY_GOAL_MANAGER_CORE_HPP_ #define EMERGENCY_GOAL_MANAGER_CORE_HPP_ -#include -#include -#include -#include - #include +#include +#include #include #include +#include +#include #include #include diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index a45782f689e0c..5fa1645961768 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -49,7 +49,6 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") void EmergencyGoalManager::onEmergencyGoals( const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg) { - std::queue emergency_goals_queue; for (const auto & goal : msg->goals) { emergency_goals_queue.push(goal); From 9e75f1bc46f591fedfdefc173bb0562bb5185998 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 4 Apr 2024 16:10:31 +0900 Subject: [PATCH 46/48] chore(mrm_pull_over_manager): change default pull over points Signed-off-by: Makoto Kurihara --- system/mrm_pull_over_manager/config/pull_over_point.csv | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/mrm_pull_over_manager/config/pull_over_point.csv b/system/mrm_pull_over_manager/config/pull_over_point.csv index e8061f25967fa..b2d5f8e99cbae 100644 --- a/system/mrm_pull_over_manager/config/pull_over_point.csv +++ b/system/mrm_pull_over_manager/config/pull_over_point.csv @@ -1,5 +1,5 @@ x,y,z,yaw,lane_id -65401.7, 684.4, 715.8, 0, 19464 +65401.7, 684.4, 715.8, 0, 56775 65626.6, 681.4, 714.0, 0, 19779 -65808.1, 720.5, 712.6, -1.3, 20976 -65570.5, 673.9, 714.5, 3.14, 20478 +65810.6, 721.1, 712.6, -1.3, 20062 +65570.5, 673.9, 714.5, 3.14, 20479 From bcbb83de1793752c47e8571d8b5f08c05f1163c7 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 4 Apr 2024 16:52:59 +0900 Subject: [PATCH 47/48] feat(emergency_goal_manager): change service for route selector Signed-off-by: Makoto Kurihara --- .../include/emergency_goal_manager_core.hpp | 10 +++++----- system/emergency_goal_manager/package.xml | 2 +- .../src/emergency_goal_manager_core.cpp | 12 ++++++------ 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp index 9f947816cd542..8dd4b24c8fa42 100644 --- a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -17,10 +17,10 @@ #include -#include -#include #include #include +#include +#include #include #include @@ -36,8 +36,8 @@ class EmergencyGoalManager : public rclcpp::Node EmergencyGoalManager(); private: - using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; - using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + using SetWaypointRoute = tier4_planning_msgs::srv::SetWaypointRoute; + using ClearRoute = tier4_planning_msgs::srv::ClearRoute; // Subscriber rclcpp::Subscription::SharedPtr @@ -51,7 +51,7 @@ class EmergencyGoalManager : public rclcpp::Node // Client rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_; - rclcpp::Client::SharedPtr client_set_mrm_route_points_; + rclcpp::Client::SharedPtr client_set_mrm_route_points_; rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_; rclcpp::Client::SharedPtr client_clear_mrm_route_; diff --git a/system/emergency_goal_manager/package.xml b/system/emergency_goal_manager/package.xml index 5b7d6e35f0bc7..576186f384312 100644 --- a/system/emergency_goal_manager/package.xml +++ b/system/emergency_goal_manager/package.xml @@ -12,9 +12,9 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs rclcpp_components std_srvs + tier4_planning_msgs tier4_system_msgs ament_lint_auto diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index 5fa1645961768..bad89052d85a8 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -30,13 +30,13 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") // Client client_set_mrm_route_points_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - client_set_mrm_route_points_ = create_client( - "/planning/mission_planning/mission_planner/srv/set_mrm_route", + client_set_mrm_route_points_ = create_client( + "/planning/mission_planning/route_selector/mrm/set_waypoint_route", rmw_qos_profile_services_default, client_set_mrm_route_points_callback_group_); client_clear_mrm_route_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client_clear_mrm_route_ = create_client( - "/planning/mission_planning/mission_planner/srv/clear_mrm_route", + "/planning/mission_planning/route_selector/mrm/clear_route", rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_); // Initialize @@ -76,10 +76,10 @@ void EmergencyGoalManager::onEmergencyGoalsClearCommand( void EmergencyGoalManager::callSetMrmRoutePoints() { - auto request = std::make_shared(); + auto request = std::make_shared(); request->header.frame_id = "map"; request->header.stamp = this->now(); - request->option.allow_goal_modification = true; + request->allow_modification = true; while (!emergency_goals_map_.empty()) { // TODO(Makoto Kurihara): set goals with the highest priority @@ -92,7 +92,7 @@ void EmergencyGoalManager::callSetMrmRoutePoints() continue; } - request->goal = goal_queue.front(); + request->goal_pose = goal_queue.front(); goal_queue.pop(); auto future = client_set_mrm_route_points_->async_send_request(request); From 7fa0cf5d562e3fa5b3c4d30e4ca0119cc70bfd77 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Apr 2024 07:55:37 +0000 Subject: [PATCH 48/48] style(pre-commit): autofix --- .../src/emergency_goal_manager_core.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp index bad89052d85a8..860e4c3bc3f6b 100644 --- a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -36,8 +36,8 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") client_clear_mrm_route_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client_clear_mrm_route_ = create_client( - "/planning/mission_planning/route_selector/mrm/clear_route", - rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_); + "/planning/mission_planning/route_selector/mrm/clear_route", rmw_qos_profile_services_default, + client_clear_mrm_route_callback_group_); // Initialize while (!client_set_mrm_route_points_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {