diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index fecee4426cc11..9ee086231ed7c 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -39,6 +39,18 @@ struct SetRoute static constexpr char name[] = "/planning/mission_planning/set_route"; }; +struct ChangeRoutePoints +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; + static constexpr char name[] = "/planning/mission_planning/change_route_points"; +}; + +struct ChangeRoute +{ + using Service = autoware_planning_msgs::srv::SetRoute; + static constexpr char name[] = "/planning/mission_planning/change_route"; +}; + struct ClearRoute { using Service = autoware_adapi_v1_msgs::srv::ClearRoute; diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index b63c008d32b64..0b7269b85763b 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/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_planning_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_planning_msgs/srv/SetRoute | route change request with lanelet waypoints | ### 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/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 56cb1fd004b7d..65bcb74886662 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_change_route_, this, &MissionPlanner::on_change_route); + adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); change_state(RouteState::Message::UNSET); } @@ -142,7 +144,7 @@ void MissionPlanner::change_state(RouteState::Message::_state_type state) pub_state_->publish(state_); } -// NOTE: The route services should be mutually exclusive by callback group. +// NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_clear_route( const ClearRoute::Service::Request::SharedPtr, const ClearRoute::Service::Response::SharedPtr res) { @@ -151,7 +153,7 @@ void MissionPlanner::on_clear_route( res->status.success = true; } -// NOTE: The route services should be mutually exclusive by callback group. +// NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_set_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res) { @@ -188,7 +190,7 @@ void MissionPlanner::on_set_route( res->status.success = true; } -// NOTE: The route services should be mutually exclusive by callback group. +// NOTE: The route interface should be mutually exclusive by callback group. void MissionPlanner::on_set_route_points( const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res) @@ -238,6 +240,25 @@ void MissionPlanner::on_set_route_points( res->status.success = true; } +// NOTE: The route interface should be mutually exclusive by callback group. +void MissionPlanner::on_change_route( + const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res) +{ + // TODO(Yutaka Shimizu): reroute + (void)req; + (void)res; +} + +// NOTE: The route interface should be mutually exclusive by callback group. +void MissionPlanner::on_change_route_points( + const SetRoutePoints::Service::Request::SharedPtr req, + const SetRoutePoints::Service::Response::SharedPtr res) +{ + // TODO(Yutaka Shimizu): reroute + (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..811c42292104d 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -41,9 +41,11 @@ using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; using MarkerArray = visualization_msgs::msg::MarkerArray; +using ClearRoute = planning_interface::ClearRoute; using SetRoutePoints = planning_interface::SetRoutePoints; using SetRoute = planning_interface::SetRoute; -using ClearRoute = planning_interface::ClearRoute; +using ChangeRoutePoints = planning_interface::ChangeRoutePoints; +using ChangeRoute = planning_interface::ChangeRoute; using Route = planning_interface::Route; using RouteState = planning_interface::RouteState; using Odometry = nav_msgs::msg::Odometry; @@ -78,6 +80,8 @@ class MissionPlanner : public rclcpp::Node component_interface_utils::Service::SharedPtr srv_clear_route_; component_interface_utils::Service::SharedPtr srv_set_route_; component_interface_utils::Service::SharedPtr srv_set_route_points_; + component_interface_utils::Service::SharedPtr srv_change_route_; + component_interface_utils::Service::SharedPtr srv_change_route_points_; void on_clear_route( const ClearRoute::Service::Request::SharedPtr req, const ClearRoute::Service::Response::SharedPtr res); @@ -87,6 +91,12 @@ class MissionPlanner : public rclcpp::Node void on_set_route_points( const SetRoutePoints::Service::Request::SharedPtr req, const SetRoutePoints::Service::Response::SharedPtr res); + void on_change_route( + const SetRoute::Service::Request::SharedPtr req, + const SetRoute::Service::Response::SharedPtr res); + void on_change_route_points( + const SetRoutePoints::Service::Request::SharedPtr req, + const SetRoutePoints::Service::Response::SharedPtr res); }; } // namespace mission_planner