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 @kosuke55
isamu-takagi authored and kosuke55 committed Apr 10, 2023
1 parent 91ac2bc commit 3ee1397
Show file tree
Hide file tree
Showing 4 changed files with 59 additions and 15 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
24 changes: 13 additions & 11 deletions planning/mission_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,11 +25,13 @@ 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

Expand All @@ -40,12 +42,12 @@ In current Autoware.universe, only Lanelet2 map format is supported.

### 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
26 changes: 23 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);
adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal);

change_state(RouteState::Message::UNSET);
Expand Down Expand Up @@ -143,7 +145,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 @@ -152,7 +154,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 @@ -189,7 +191,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 @@ -246,6 +248,24 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt
arrival_checker_.modify_goal(*msg);
}

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 @@ -42,9 +42,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 @@ -79,6 +81,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 @@ -91,6 +95,12 @@ class MissionPlanner : public rclcpp::Node

component_interface_utils::Subscription<ModifiedGoal>::SharedPtr sub_modified_goal_;
void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg);
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 3ee1397

Please sign in to comment.