Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(mission_planner): add reroute interface for mrm #3299

Merged
merged 7 commits into from
Apr 18, 2023
Merged

feat(mission_planner): add reroute interface for mrm #3299

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
610e435
feat(mission_planner): add reroute interface for mrm
isamu-takagi Apr 6, 2023
9d8573f
feat: fix readme
isamu-takagi Apr 10, 2023
2053a14
Merge branch 'main' into feat/mission-planner-reroute-mrm
isamu-takagi Apr 10, 2023
4605211
feat: rename mrm interface
isamu-takagi Apr 17, 2023
0d0442e
Merge branch 'main' into feat/mission-planner-reroute-mrm
isamu-takagi Apr 17, 2023
f02299e
feat: rename mrm interface
isamu-takagi Apr 18, 2023
aa320bc
feat: change route service type
isamu-takagi Apr 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 15 additions & 13 deletions planning/mission_planner/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +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 |
| 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 |

### Subscriptions

Expand All @@ -42,12 +44,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/mission_planning/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state |
| `/planning/mission_planning/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
14 changes: 14 additions & 0 deletions planning/mission_planner/include/mission_planner/mission_planner_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,24 @@
#include <rclcpp/qos.hpp>

#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
#include <autoware_planning_msgs/srv/set_pose_with_uuid_stamped.hpp>
#include <std_srvs/srv/trigger.hpp>

namespace mission_planner
{

struct SetMrmRoute
{
using Service = autoware_planning_msgs::srv::SetPoseWithUuidStamped;
static constexpr char name[] = "~/srv/set_mrm_route";
};

struct ClearMrmRoute
{
using Service = std_srvs::srv::Trigger;
static constexpr char name[] = "~/srv/clear_mrm_route";
};

struct ModifiedGoal
{
using Message = autoware_planning_msgs::msg::PoseWithUuidStamped;
Expand Down
1 change: 1 addition & 0 deletions planning/mission_planner/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>route_handler</depend>
<depend>std_srvs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_autoware_utils</depend>
Expand Down
21 changes: 21 additions & 0 deletions planning/mission_planner/src/mission_planner/mission_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
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_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);
Expand Down Expand Up @@ -247,6 +249,25 @@ void MissionPlanner::on_set_route_points(
}

// NOTE: The route interface should be mutually exclusive by callback group.
void MissionPlanner::on_set_mrm_route(
const SetMrmRoute::Service::Request::SharedPtr req,
const SetMrmRoute::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_route(
const ClearMrmRoute::Service::Request::SharedPtr req,
const ClearMrmRoute::Service::Response::SharedPtr res)
{
// TODO(Yutaka Shimizu): reroute for MRM
(void)req;
(void)res;
}

void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg)
{
// TODO(Yutaka Shimizu): reroute if the goal is outside the lane.
Expand Down
9 changes: 9 additions & 0 deletions planning/mission_planner/src/mission_planner/mission_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,15 @@ class MissionPlanner : public rclcpp::Node
const SetRoutePoints::Service::Request::SharedPtr req,
const SetRoutePoints::Service::Response::SharedPtr res);

component_interface_utils::Service<SetMrmRoute>::SharedPtr srv_set_mrm_route_;
component_interface_utils::Service<ClearMrmRoute>::SharedPtr srv_clear_mrm_route_;
void on_set_mrm_route(
const SetMrmRoute::Service::Request::SharedPtr req,
const SetMrmRoute::Service::Response::SharedPtr res);
void on_clear_mrm_route(
const ClearMrmRoute::Service::Request::SharedPtr req,
const ClearMrmRoute::Service::Response::SharedPtr res);

component_interface_utils::Subscription<ModifiedGoal>::SharedPtr sub_modified_goal_;
void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg);
void on_change_route(
Expand Down