diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 70633869c6fc5..1e43464ee954d 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -92,6 +92,12 @@ + + + + + + diff --git a/system/emergency_goal_manager/CMakeLists.txt b/system/emergency_goal_manager/CMakeLists.txt new file mode 100644 index 0000000000000..834ed25931e14 --- /dev/null +++ b/system/emergency_goal_manager/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(emergency_goal_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(emergency_goal_manager + src/emergency_goal_manager_node.cpp + src/emergency_goal_manager_core.cpp +) + +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..c27ed818a50a4 --- /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..e69de29bb2d1d diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp new file mode 100644 index 0000000000000..6d6db5ea9ca59 --- /dev/null +++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp @@ -0,0 +1,70 @@ +// 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_CORE_HPP_ +#define EMERGENCY_GOAL_MANAGER_CORE_HPP_ + +// Autoware +#include +#include +#include +#include + +// ROS 2 core +#include + +#include +#include + +#include +#include +#include + +namespace emergency_goal_manager +{ +class EmergencyGoalManager : public rclcpp::Node +{ +public: + EmergencyGoalManager(); + +private: + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + + // 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); + + // 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_; + + // Variables + std::unordered_map> emergency_goals_map_; + + // Algorithm + void callSetMrmRoutePoints(); + void callClearMrmRoute(); +}; +} // namespace emergency_goal_manager + +#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 new file mode 100644 index 0000000000000..1d85c1172d06b --- /dev/null +++ b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/emergency_goal_manager/package.xml b/system/emergency_goal_manager/package.xml new file mode 100644 index 0000000000000..5b7d6e35f0bc7 --- /dev/null +++ b/system/emergency_goal_manager/package.xml @@ -0,0 +1,26 @@ + + + + emergency_goal_manager + 0.1.0 + The emergency goal manager package + Makoto Kurihara + Tomohito Ando + Ryuta Kambe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + rclcpp_components + std_srvs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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..07cf8d895c454 --- /dev/null +++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp @@ -0,0 +1,152 @@ +// 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.frame_id = "map"; + 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) { + RCLCPP_WARN(get_logger(), "MRM Route service timeout."); + continue; + } else { + 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."); + std::this_thread::sleep_for(std::chrono::seconds(1)); + continue; + } + } + } + + callClearMrmRoute(); +} + +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()) { + if (std::chrono::steady_clock::now() - start_time > duration) { + RCLCPP_WARN(get_logger(), "Clear MRM Route operation 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; + } else { + if (future.get()->status.success) { + 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; + } + } + } +} +} // namespace emergency_goal_manager 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..c307bbf70ef57 --- /dev/null +++ b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp @@ -0,0 +1,28 @@ +// 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 "emergency_goal_manager_core.hpp" + +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; +} 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..23a6ed29442dc --- /dev/null +++ b/system/mrm_pull_over_manager/README.md @@ -0,0 +1,23 @@ +# 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..1d988cc09ece8 --- /dev/null +++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate: 10.0 + max_goal_pose_num: 3 + yaw_deviation_threshold: 0.5 # [rad] + margin_time_to_goal: 10.0 # [sec] 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..e8061f25967fa --- /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 +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 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..dc844d2dc8383 --- /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: + explicit 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_ 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..52bd0b21ecca3 --- /dev/null +++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/mrm_pull_over_manager_core.hpp @@ -0,0 +1,146 @@ +// 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_ + +// C++ +#include +#include +#include +#include + +// ROS 2 +#include + +#include + +// Autoware +#include +#include + +#include +#include +#include +#include +#include +#include + +// lanelet +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace mrm_pull_over_manager +{ +class MrmPullOverManager : public rclcpp::Node +{ +public: + MrmPullOverManager(); + +private: + using Pose = geometry_msgs::msg::Pose; + 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 MrmBehaviorStatus = tier4_system_msgs::msg::MrmBehaviorStatus; + using EmergencyGoalsClearCommand = tier4_system_msgs::msg::EmergencyGoalsClearCommand; + using EmergencyGoalsStamped = tier4_system_msgs::msg::EmergencyGoalsStamped; + + struct Parameter + { + double update_rate; + std::string pull_over_point_file_path; + size_t max_goal_pose_num; + double yaw_deviation_threshold; + double margin_time_to_goal; + }; + + // Subscribtoers + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + Odometry::ConstSharedPtr odom_; + LaneletRoute::ConstSharedPtr route_; + 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); + + // Server + rclcpp::Service::SharedPtr operate_mrm_; + + 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_emergency_goals_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_emergency_goals_clear_command_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void on_timer(); + + // Parameters + Parameter param_; + + // Variables + PoseLaneIdMap candidate_goals_; + MrmBehaviorStatus status_; + + // Algorithm + bool is_data_ready(); + 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; + + /** + * @brief Find the goals within the lanelet and publish them + */ + bool find_goals_within_route(); + + /** + * @brief Find the goals that have the same lanelet id with the candidate_lanelets + * @param candidate_lanelets + * @return + */ + 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); +}; +} // 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 new file mode 100644 index 0000000000000..6828ca854a8c5 --- /dev/null +++ b/system/mrm_pull_over_manager/launch/mrm_pull_over_manager.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/mrm_pull_over_manager/package.xml b/system/mrm_pull_over_manager/package.xml new file mode 100644 index 0000000000000..2509c2d90d48f --- /dev/null +++ b/system/mrm_pull_over_manager/package.xml @@ -0,0 +1,31 @@ + + + + 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_auto_system_msgs + lanelet2_extension + motion_utils + nav_msgs + rclcpp + route_handler + std_msgs + std_srvs + tier4_system_msgs + + 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..09c5f9cbbe196 --- /dev/null +++ b/system/mrm_pull_over_manager/src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp @@ -0,0 +1,335 @@ +// 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 "mrm_pull_over_manager/csv_parser.hpp" +namespace mrm_pull_over_manager +{ +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.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; +} + +/** + * @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 autoware_planning_msgs::msg::LaneletRoute & route, + const lanelet::ConstLanelet & start_lanelet) +{ + lanelet::ConstLanelets result_lanelets; + 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 + } + } + } + + // search lanelets from the start_lanelet + if (!found_start_lane) { + continue; + } + + const auto current_lane = route_handler.getLaneletsFromId(segment.preferred_primitive.id); + result_lanelets.emplace_back(current_lane); + 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); + result_lanelets.insert(result_lanelets.end(), left_lanes.begin(), left_lanes.end()); + } + + return result_lanelets; +} +} // namespace lanelet_util + +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"); + param_.margin_time_to_goal = declare_parameter("margin_time_to_goal"); + + using std::placeholders::_1; + using std::placeholders::_2; + + // Subscriber + sub_odom_ = create_subscription( + "~/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/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_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( + "~/output/mrm/pull_over/goals_clear_command", rclcpp::QoS{1}); + + // Server + 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(); + 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; +} + +std::string MrmPullOverManager::get_module_name() const +{ + return "mrm_pull_over_manager"; +} + +void MrmPullOverManager::on_timer() +{ + publish_status(); +} + +void MrmPullOverManager::publish_status() const +{ + auto status = status_; + status.stamp = this->now(); + pub_status_->publish(status); +} + +void MrmPullOverManager::publish_emergency_goals(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::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); +} + +void MrmPullOverManager::operateMrm( + const tier4_system_msgs::srv::OperateMrm::Request::SharedPtr request, + const tier4_system_msgs::srv::OperateMrm::Response::SharedPtr response) +{ + if (request->operate == true) { + const bool result = find_goals_within_route(); + response->response.success = result; + status_.state = MrmBehaviorStatus::OPERATING; + } + + if (request->operate == false) { + publish_emergency_goals_clear_command(); + response->response.success = true; + status_.state = MrmBehaviorStatus::AVAILABLE; + } +} + +void MrmPullOverManager::on_odometry(const Odometry::ConstSharedPtr msg) +{ + odom_ = 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); +} + +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_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for odometry msg..."); + return false; + } + + if (!route_) { + RCLCPP_WARN_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."); + return false; + } + + if (!trajectory_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for trajectory."); + return false; + } + + return true; +} + +bool MrmPullOverManager::find_goals_within_route() +{ + if (!is_data_ready()) { + return false; + } + + const auto current_lanelet = lanelet_util::get_current_lanelet(route_handler_, odom_->pose.pose); + 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); + + const auto emergency_goals = find_goals_in_lanelets(candidate_lanelets); + + const auto filtered_emergency_goals = filter_nearby_goals(emergency_goals); + + publish_emergency_goals(filtered_emergency_goals); + + return true; +} + +std::vector MrmPullOverManager::find_goals_in_lanelets( + const lanelet::ConstLanelets & candidate_lanelets) const +{ + RCLCPP_DEBUG(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.emplace_back(it->second); + } + + if (goals.size() >= param_.max_goal_pose_num) { + break; + } + } + + return goals; +} + +std::vector MrmPullOverManager::filter_nearby_goals( + const std::vector & poses) +{ + 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_DEBUG(this->get_logger(), "yaw deviation to pose: %lf", yaw_deviation); + if (std::abs(yaw_deviation) > param_.yaw_deviation_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 + // 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_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; + } + } + + // 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 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..8aa5ac1462690 --- /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; +}