From 610e43576c638a33fb98e5fd141b70ceaf36f779 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 6 Apr 2023 20:07:46 +0900 Subject: [PATCH 1/5] feat(mission_planner): add reroute interface for mrm Signed-off-by: Takagi, Isamu --- planning/mission_planner/README.md | 32 ++++++++------- .../mission_planner_interface.hpp | 40 +++++++++++++++++++ planning/mission_planner/package.xml | 1 + .../src/mission_planner/mission_planner.cpp | 22 ++++++++++ .../src/mission_planner/mission_planner.hpp | 10 +++++ 5 files changed, 90 insertions(+), 15 deletions(-) create mode 100644 planning/mission_planner/include/mission_planner/mission_planner_interface.hpp diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index b63c008d32b64..a8e7c1a4efe75 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -25,27 +25,29 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Services -| Name | Type | Description | -| ------------------------------------ | ------------------------------------------- | --------------------------------- | -| `/planning/routing/clear_route` | autoware_adapi_v1_msgs::srv::ClearRoute | route clear request | -| `/planning/routing/set_route_points` | autoware_adapi_v1_msgs::srv::SetRoutePoints | route request with pose waypoints | -| `/planning/routing/set_route` | autoware_planning_msgs::srv::SetRoute | route request with HAD map format | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------- | --------------------------------- | +| `/planning/routing/clear_route` | autoware_adapi_v1_msgs/srv/ClearRoute | route clear request | +| `/planning/routing/set_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints | +| `/planning/routing/set_route` | autoware_planning_msgs/srv/SetRoute | route request with HAD map format | +| `~/srv/set_mrm` | autoware_planning_msgs/srv/SetPoseWithUuidStamped | set emergency route | +| `~/srv/clear_mrm` | std_srvs/srv/Trigger | clear emergency route | ### Subscriptions -| Name | Type | Description | -| --------------------- | ------------------------------------ | --------------------------- | -| `input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | -| `input/modified_goal` | geometry_msgs/PoseStamped | goal pose for arrival check | +| Name | Type | Description | +| --------------------- | ------------------------------------ | ---------------------- | +| `input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | ### Publications -| Name | Type | Description | -| ------------------------------- | --------------------------------------- | ------------------------ | -| `/planning/routing/route_state` | autoware_adapi_v1_msgs::msg::RouteState | route state | -| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | -| `debug/route_marker` | visualization_msgs::msg::MarkerArray | route marker for debug | -| `debug/goal_footprint` | visualization_msgs::msg::MarkerArray | goal footprint for debug | +| Name | Type | Description | +| ------------------------------- | ------------------------------------- | ------------------------ | +| `/planning/routing/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state | +| `/planning/routing/route` | autoware_planning_msgs/LaneletRoute | route | +| `debug/route_marker` | visualization_msgs/msg/MarkerArray | route marker for debug | +| `debug/goal_footprint` | visualization_msgs/msg/MarkerArray | goal footprint for debug | ## Route section diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp new file mode 100644 index 0000000000000..3d070cbd46b58 --- /dev/null +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -0,0 +1,40 @@ +// 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 MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ +#define MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ + +#include + +#include +#include + +namespace mission_planner +{ + +struct SetMrmGoal +{ + using Service = autoware_planning_msgs::srv::SetPoseWithUuidStamped; + static constexpr char name[] = "~/srv/set_mrm_goal"; +}; + +struct ClearMrmGoal +{ + using Service = std_srvs::srv::Trigger; + static constexpr char name[] = "~/srv/clear_mrm_goal"; +}; + +} // namespace mission_planner + +#endif // MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f972c18b66d0d..12166c56c0761 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -28,6 +28,7 @@ rclcpp rclcpp_components route_handler + std_srvs tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 56cb1fd004b7d..b8e2441df8617 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -85,6 +85,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_clear_route_, this, &MissionPlanner::on_clear_route); adaptor.init_srv(srv_set_route_, this, &MissionPlanner::on_set_route); adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points); + adaptor.init_srv(srv_set_mrm_goal_, this, &MissionPlanner::on_set_mrm_goal); + adaptor.init_srv(srv_clear_mrm_goal_, this, &MissionPlanner::on_clear_mrm_goal); change_state(RouteState::Message::UNSET); } @@ -238,6 +240,26 @@ void MissionPlanner::on_set_route_points( res->status.success = true; } +// NOTE: The route interface should be mutually exclusive by callback group. +void MissionPlanner::on_set_mrm_goal( + const SetMrmGoal::Service::Request::SharedPtr req, + const SetMrmGoal::Service::Response::SharedPtr res) +{ + // TODO(Yutaka Shimizu): reroute for MRM + (void)req; + (void)res; +} + +// NOTE: The route interface should be mutually exclusive by callback group. +void MissionPlanner::on_clear_mrm_goal( + const ClearMrmGoal::Service::Request::SharedPtr req, + const ClearMrmGoal::Service::Response::SharedPtr res) +{ + // TODO(Yutaka Shimizu): reroute for MRM + (void)req; + (void)res; +} + } // namespace mission_planner #include diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 2907b4698e156..e81760e4e8fa0 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -87,6 +88,15 @@ class MissionPlanner : public rclcpp::Node void on_set_route_points( const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res); + + component_interface_utils::Service::SharedPtr srv_set_mrm_goal_; + component_interface_utils::Service::SharedPtr srv_clear_mrm_goal_; + void on_set_mrm_goal( + const SetMrmGoal::Service::Request::SharedPtr req, + const SetMrmGoal::Service::Response::SharedPtr res); + void on_clear_mrm_goal( + const ClearMrmGoal::Service::Request::SharedPtr req, + const ClearMrmGoal::Service::Response::SharedPtr res); }; } // namespace mission_planner From 9d8573f087e8d96536a85e81fa729e53ef8c5b36 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 10 Apr 2023 15:11:29 +0900 Subject: [PATCH 2/5] feat: fix readme Signed-off-by: Takagi, Isamu --- planning/mission_planner/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index a8e7c1a4efe75..0822466ae6e91 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -30,8 +30,8 @@ In current Autoware.universe, only Lanelet2 map format is supported. | `/planning/routing/clear_route` | autoware_adapi_v1_msgs/srv/ClearRoute | route clear request | | `/planning/routing/set_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints | | `/planning/routing/set_route` | autoware_planning_msgs/srv/SetRoute | route request with HAD map format | -| `~/srv/set_mrm` | autoware_planning_msgs/srv/SetPoseWithUuidStamped | set emergency route | -| `~/srv/clear_mrm` | std_srvs/srv/Trigger | clear emergency route | +| `~/srv/set_mrm_goal` | autoware_planning_msgs/srv/SetPoseWithUuidStamped | set emergency route | +| `~/srv/clear_mrm_goal` | std_srvs/srv/Trigger | clear emergency route | ### Subscriptions From 4605211fda153943b2ba1071990634079d1c3e01 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 17 Apr 2023 16:08:53 +0900 Subject: [PATCH 3/5] feat: rename mrm interface Signed-off-by: Takagi, Isamu --- planning/mission_planner/README.md | 4 ++-- .../include/mission_planner/mission_planner_interface.hpp | 4 ++-- .../src/mission_planner/mission_planner.cpp | 8 ++++---- .../src/mission_planner/mission_planner.hpp | 8 ++++---- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 0822466ae6e91..e1bed2237bb60 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -30,8 +30,8 @@ In current Autoware.universe, only Lanelet2 map format is supported. | `/planning/routing/clear_route` | autoware_adapi_v1_msgs/srv/ClearRoute | route clear request | | `/planning/routing/set_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints | | `/planning/routing/set_route` | autoware_planning_msgs/srv/SetRoute | route request with HAD map format | -| `~/srv/set_mrm_goal` | autoware_planning_msgs/srv/SetPoseWithUuidStamped | set emergency route | -| `~/srv/clear_mrm_goal` | std_srvs/srv/Trigger | clear emergency route | +| `~/srv/set_mrm_route` | autoware_planning_msgs/srv/SetPoseWithUuidStamped | set emergency route | +| `~/srv/clear_mrm_route` | std_srvs/srv/Trigger | clear emergency route | ### Subscriptions 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 d9ed77f1363f4..b1776423e39b4 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -27,13 +27,13 @@ namespace mission_planner struct SetMrmGoal { using Service = autoware_planning_msgs::srv::SetPoseWithUuidStamped; - static constexpr char name[] = "~/srv/set_mrm_goal"; + static constexpr char name[] = "~/srv/set_mrm_route"; }; struct ClearMrmGoal { using Service = std_srvs::srv::Trigger; - static constexpr char name[] = "~/srv/clear_mrm_goal"; + static constexpr char name[] = "~/srv/clear_mrm_route"; }; struct ModifiedGoal diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 5c37f0499b2cd..a827066362856 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -85,8 +85,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_clear_route_, this, &MissionPlanner::on_clear_route); adaptor.init_srv(srv_set_route_, this, &MissionPlanner::on_set_route); adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points); - adaptor.init_srv(srv_set_mrm_goal_, this, &MissionPlanner::on_set_mrm_goal); - adaptor.init_srv(srv_clear_mrm_goal_, this, &MissionPlanner::on_clear_mrm_goal); + adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); + adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); change_state(RouteState::Message::UNSET); @@ -242,7 +242,7 @@ void MissionPlanner::on_set_route_points( } // NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_set_mrm_goal( +void MissionPlanner::on_set_mrm_route( const SetMrmGoal::Service::Request::SharedPtr req, const SetMrmGoal::Service::Response::SharedPtr res) { @@ -252,7 +252,7 @@ void MissionPlanner::on_set_mrm_goal( } // NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_clear_mrm_goal( +void MissionPlanner::on_clear_mrm_route( const ClearMrmGoal::Service::Request::SharedPtr req, const ClearMrmGoal::Service::Response::SharedPtr res) { diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index a7ecfff0fd8f6..6f03f4dc898a8 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -89,12 +89,12 @@ class MissionPlanner : public rclcpp::Node const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res); - component_interface_utils::Service::SharedPtr srv_set_mrm_goal_; - component_interface_utils::Service::SharedPtr srv_clear_mrm_goal_; - void on_set_mrm_goal( + component_interface_utils::Service::SharedPtr srv_set_mrm_route_; + component_interface_utils::Service::SharedPtr srv_clear_mrm_route_; + void on_set_mrm_route( const SetMrmGoal::Service::Request::SharedPtr req, const SetMrmGoal::Service::Response::SharedPtr res); - void on_clear_mrm_goal( + void on_clear_mrm_route( const ClearMrmGoal::Service::Request::SharedPtr req, const ClearMrmGoal::Service::Response::SharedPtr res); From f02299e57941cc227df9013fa984cb59f9e3a504 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 18 Apr 2023 11:16:25 +0900 Subject: [PATCH 4/5] feat: rename mrm interface Signed-off-by: Takagi, Isamu --- .../mission_planner/mission_planner_interface.hpp | 4 ++-- .../src/mission_planner/mission_planner.cpp | 8 ++++---- .../src/mission_planner/mission_planner.hpp | 12 ++++++------ 3 files changed, 12 insertions(+), 12 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 b1776423e39b4..5897c0232c167 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -24,13 +24,13 @@ namespace mission_planner { -struct SetMrmGoal +struct SetMrmRoute { using Service = autoware_planning_msgs::srv::SetPoseWithUuidStamped; static constexpr char name[] = "~/srv/set_mrm_route"; }; -struct ClearMrmGoal +struct ClearMrmRoute { using Service = std_srvs::srv::Trigger; 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 d6a8dac113c3a..5b8954158192b 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -250,8 +250,8 @@ void MissionPlanner::on_set_route_points( // NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_set_mrm_route( - const SetMrmGoal::Service::Request::SharedPtr req, - const SetMrmGoal::Service::Response::SharedPtr res) + const SetMrmRoute::Service::Request::SharedPtr req, + const SetMrmRoute::Service::Response::SharedPtr res) { // TODO(Yutaka Shimizu): reroute for MRM (void)req; @@ -260,8 +260,8 @@ void MissionPlanner::on_set_mrm_route( // NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_clear_mrm_route( - const ClearMrmGoal::Service::Request::SharedPtr req, - const ClearMrmGoal::Service::Response::SharedPtr res) + const ClearMrmRoute::Service::Request::SharedPtr req, + const ClearMrmRoute::Service::Response::SharedPtr res) { // TODO(Yutaka Shimizu): reroute for MRM (void)req; diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index a8a8f960d75dd..21a4339e19715 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -93,14 +93,14 @@ class MissionPlanner : public rclcpp::Node const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res); - component_interface_utils::Service::SharedPtr srv_set_mrm_route_; - component_interface_utils::Service::SharedPtr srv_clear_mrm_route_; + component_interface_utils::Service::SharedPtr srv_set_mrm_route_; + component_interface_utils::Service::SharedPtr srv_clear_mrm_route_; void on_set_mrm_route( - const SetMrmGoal::Service::Request::SharedPtr req, - const SetMrmGoal::Service::Response::SharedPtr res); + const SetMrmRoute::Service::Request::SharedPtr req, + const SetMrmRoute::Service::Response::SharedPtr res); void on_clear_mrm_route( - const ClearMrmGoal::Service::Request::SharedPtr req, - const ClearMrmGoal::Service::Response::SharedPtr res); + const ClearMrmRoute::Service::Request::SharedPtr req, + const ClearMrmRoute::Service::Response::SharedPtr res); component_interface_utils::Subscription::SharedPtr sub_modified_goal_; void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); From aa320bc7f6b55f00e38f66ca973d25448f760a20 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 18 Apr 2023 11:53:37 +0900 Subject: [PATCH 5/5] feat: change route service type Signed-off-by: Takagi, Isamu --- planning/mission_planner/README.md | 18 +++++++++--------- .../mission_planner_interface.hpp | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 3d0f837558104..2ea8b96b1cfd2 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -25,15 +25,15 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Services -| Name | Type | Description | -| ------------------------------------------------ | ------------------------------------------------- | ------------------------------------------- | -| `/planning/mission_planning/clear_route` | autoware_adapi_v1_msgs/srv/ClearRoute | route clear request | -| `/planning/mission_planning/set_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints | -| `/planning/mission_planning/set_route` | autoware_adapi_v1_msgs/srv/SetRoute | route request with lanelet waypoints | -| `/planning/mission_planning/change_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route change request with pose waypoints | -| `/planning/mission_planning/change_route` | autoware_adapi_v1_msgs/srv/SetRoute | route change request with lanelet waypoints | -| `~/srv/set_mrm_route` | autoware_planning_msgs/srv/SetPoseWithUuidStamped | set emergency route | -| `~/srv/clear_mrm_route` | std_srvs/srv/Trigger | clear emergency route | +| Name | Type | Description | +| ------------------------------------------------ | ----------------------------------------- | ------------------------------------------- | +| `/planning/mission_planning/clear_route` | autoware_adapi_v1_msgs/srv/ClearRoute | route clear request | +| `/planning/mission_planning/set_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route request with pose waypoints | +| `/planning/mission_planning/set_route` | autoware_adapi_v1_msgs/srv/SetRoute | route request with lanelet waypoints | +| `/planning/mission_planning/change_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route change request with pose waypoints | +| `/planning/mission_planning/change_route` | autoware_adapi_v1_msgs/srv/SetRoute | route change request with lanelet waypoints | +| `~/srv/set_mrm_route` | autoware_adapi_v1_msgs/srv/SetRoutePoints | set emergency route | +| `~/srv/clear_mrm_route` | std_srvs/srv/Trigger | clear emergency route | ### Subscriptions 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 5897c0232c167..916dabfdbba2b 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp @@ -17,8 +17,8 @@ #include +#include #include -#include #include namespace mission_planner @@ -26,7 +26,7 @@ namespace mission_planner struct SetMrmRoute { - using Service = autoware_planning_msgs::srv::SetPoseWithUuidStamped; + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; static constexpr char name[] = "~/srv/set_mrm_route"; };