Skip to content

Commit

Permalink
feat(mission_planner): add reroute interface for api
Browse files Browse the repository at this point in the history 
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
  • Loading branch information
@isamu-takagi
isamu-takagi committed Apr 6, 2023
1 parent cfecdf8 commit 5c11e44
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 19 deletions.
12 changes: 12 additions & 0 deletions common/component_interface_specs/include/component_interface_specs/planning.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
32 changes: 17 additions & 15 deletions planning/mission_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
27 changes: 24 additions & 3 deletions planning/mission_planner/src/mission_planner/mission_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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 <rclcpp_components/register_node_macro.hpp>
Expand Down
12 changes: 11 additions & 1 deletion planning/mission_planner/src/mission_planner/mission_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -78,6 +80,8 @@ class MissionPlanner : public rclcpp::Node
component_interface_utils::Service<ClearRoute>::SharedPtr srv_clear_route_;
component_interface_utils::Service<SetRoute>::SharedPtr srv_set_route_;
component_interface_utils::Service<SetRoutePoints>::SharedPtr srv_set_route_points_;
component_interface_utils::Service<ChangeRoute>::SharedPtr srv_change_route_;
component_interface_utils::Service<ChangeRoutePoints>::SharedPtr srv_change_route_points_;
void on_clear_route(
const ClearRoute::Service::Request::SharedPtr req,
const ClearRoute::Service::Response::SharedPtr res);
Expand All @@ -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
Expand Down

0 comments on commit 5c11e44

Please sign in to comment.