forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(mrm_pull_over_manager): add mrm pull over
Signed-off-by: Makoto Kurihara <[email protected]>
- Loading branch information
Showing
19 changed files
with
1,068 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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. |
Empty file.
70 changes: 70 additions & 0 deletions
70
system/emergency_goal_manager/include/emergency_goal_manager_core.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <autoware_adapi_v1_msgs/srv/clear_route.hpp> | ||
#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp> | ||
#include <tier4_system_msgs/msg/emergency_goals_clear_command.hpp> | ||
#include <tier4_system_msgs/msg/emergency_goals_stamped.hpp> | ||
|
||
// ROS 2 core | ||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <geometry_msgs/msg/pose.hpp> | ||
#include <std_srvs/srv/trigger.hpp> | ||
|
||
#include <queue> | ||
#include <string> | ||
#include <unordered_map> | ||
|
||
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<tier4_system_msgs::msg::EmergencyGoalsStamped>::SharedPtr | ||
sub_emergency_goals_; | ||
rclcpp::Subscription<tier4_system_msgs::msg::EmergencyGoalsClearCommand>::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<SetRoutePoints>::SharedPtr client_set_mrm_route_points_; | ||
rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_; | ||
rclcpp::Client<ClearRoute>::SharedPtr client_clear_mrm_route_; | ||
|
||
// Variables | ||
std::unordered_map<std::string, std::queue<geometry_msgs::msg::Pose>> emergency_goals_map_; | ||
|
||
// Algorithm | ||
void callSetMrmRoutePoints(); | ||
void callClearMrmRoute(); | ||
}; | ||
} // namespace emergency_goal_manager | ||
|
||
#endif // EMERGENCY_GOAL_MANAGER_CORE_HPP_ |
9 changes: 9 additions & 0 deletions
9
system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,9 @@ | ||
<launch> | ||
<arg name="input_emergency_goals" default="/system/mrm/pull_over_manager/emergency_goals"/> | ||
<arg name="input_goals_clear_command" default="/system/mrm/pull_over_manager/goals_clear_command"/> | ||
|
||
<node pkg="emergency_goal_manager" exec="emergency_goal_manager" name="emergency_goal_manager" output="screen"> | ||
<remap from="~/input/emergency_goals" to="$(var input_emergency_goals)"/> | ||
<remap from="~/input/emergency_goals_clear_command" to="$(var input_goals_clear_command)"/> | ||
</node> | ||
</launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
<?xml version="1.0"?> | ||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
<package format="3"> | ||
<name>emergency_goal_manager</name> | ||
<version>0.1.0</version> | ||
<description>The emergency goal manager package</description> | ||
<maintainer email="[email protected]">Makoto Kurihara</maintainer> | ||
<maintainer email="[email protected]">Tomohito Ando</maintainer> | ||
<maintainer email="[email protected]">Ryuta Kambe</maintainer> | ||
<license>Apache License 2.0</license> | ||
|
||
<buildtool_depend>ament_cmake_auto</buildtool_depend> | ||
<buildtool_depend>autoware_cmake</buildtool_depend> | ||
|
||
<depend>autoware_adapi_v1_msgs</depend> | ||
<depend>rclcpp_components</depend> | ||
<depend>std_srvs</depend> | ||
<depend>tier4_system_msgs</depend> | ||
|
||
<test_depend>ament_lint_auto</test_depend> | ||
<test_depend>autoware_lint_common</test_depend> | ||
|
||
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> |
152 changes: 152 additions & 0 deletions
152
system/emergency_goal_manager/src/emergency_goal_manager_core.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <emergency_goal_manager_core.hpp> | ||
|
||
namespace emergency_goal_manager | ||
{ | ||
EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager") | ||
{ | ||
// Subscriber | ||
sub_emergency_goals_ = create_subscription<tier4_system_msgs::msg::EmergencyGoalsStamped>( | ||
"~/input/emergency_goals", rclcpp::QoS{1}, | ||
std::bind(&EmergencyGoalManager::onEmergencyGoals, this, std::placeholders::_1)); | ||
sub_emergency_goals_clear_command_ = | ||
create_subscription<tier4_system_msgs::msg::EmergencyGoalsClearCommand>( | ||
"~/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<SetRoutePoints>( | ||
"/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<ClearRoute>( | ||
"/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<geometry_msgs::msg::Pose> 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<SetRoutePoints::Request>(); | ||
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<double, std::ratio<1>>(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<ClearRoute::Request>(); | ||
const auto duration = std::chrono::duration<double, std::ratio<1>>(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 |
28 changes: 28 additions & 0 deletions
28
system/emergency_goal_manager/src/emergency_goal_manager_node.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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<emergency_goal_manager::EmergencyGoalManager>(); | ||
executor.add_node(node); | ||
executor.spin(); | ||
executor.remove_node(node); | ||
rclcpp::shutdown(); | ||
|
||
return 0; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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. |
Oops, something went wrong.