Skip to content

Commit

Permalink
feat(mission_planner): add mrm reroute (#3821)
Browse files Browse the repository at this point in the history
  • Loading branch information
@purewater0901
purewater0901 authored May 26, 2023
1 parent 712c11c commit 358d4a7
Show file tree
Hide file tree
Showing 2 changed files with 188 additions and 23 deletions.
198 changes: 176 additions & 22 deletions planning/mission_planner/src/mission_planner/mission_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,10 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"),
tf_buffer_(get_clock()),
tf_listener_(tf_buffer_),
is_emergency_{false},
original_route_(nullptr),
normal_route_(nullptr)
normal_route_(nullptr),
mrm_route_{nullptr}
{
map_frame_ = declare_parameter<std::string>("map_frame");
reroute_time_threshold_ = declare_parameter<double>("reroute_time_threshold");
Expand Down Expand Up @@ -154,6 +156,11 @@ void MissionPlanner::clear_route()
// TODO(Takagi, Isamu): publish an empty route here
}

void MissionPlanner::clear_mrm_route()
{
mrm_route_ = nullptr;
}

void MissionPlanner::change_route(const LaneletRoute & route)
{
PoseWithUuidStamped goal;
Expand All @@ -170,43 +177,64 @@ void MissionPlanner::change_route(const LaneletRoute & route)
normal_route_ = std::make_shared<LaneletRoute>(route);
}

LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req)
void MissionPlanner::change_mrm_route(const LaneletRoute & route)
{
PoseWithUuidStamped goal;
goal.header = route.header;
goal.pose = route.goal_pose;
goal.uuid = route.uuid;

arrival_checker_.set_goal(goal);
pub_route_->publish(route);
pub_marker_->publish(planner_->visualize(route));
planner_->updateRoute(route);

// update emergency route
mrm_route_ = std::make_shared<LaneletRoute>(route);
}

LaneletRoute MissionPlanner::create_route(
const std_msgs::msg::Header & header,
const std::vector<autoware_adapi_v1_msgs::msg::RouteSegment> & route_segments,
const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification)
{
PoseStamped goal_pose;
goal_pose.header = req->header;
goal_pose.pose = req->goal;
PoseStamped goal_pose_stamped;
goal_pose_stamped.header = header;
goal_pose_stamped.pose = goal_pose;

// Convert route.
LaneletRoute route;
route.start_pose = odometry_->pose.pose;
route.goal_pose = transform_pose(goal_pose).pose;
for (const auto & segment : req->segments) {
route.goal_pose = transform_pose(goal_pose_stamped).pose;
for (const auto & segment : route_segments) {
route.segments.push_back(convert(segment));
}
route.header.stamp = req->header.stamp;
route.header.stamp = header.stamp;
route.header.frame_id = map_frame_;
route.uuid.uuid = generate_random_id();
route.allow_modification = req->option.allow_goal_modification;
route.allow_modification = allow_goal_modification;

return route;
}

LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req)
LaneletRoute MissionPlanner::create_route(
const std_msgs::msg::Header & header, const std::vector<geometry_msgs::msg::Pose> & waypoints,
const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification)
{
using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response;

// Use temporary pose stamped for transform.
PoseStamped pose;
pose.header = req->header;
pose.header = header;

// Convert route points.
PlannerPlugin::RoutePoints points;
points.push_back(odometry_->pose.pose);
for (const auto & waypoint : req->waypoints) {
for (const auto & waypoint : waypoints) {
pose.pose = waypoint;
points.push_back(transform_pose(pose).pose);
}
pose.pose = req->goal;
pose.pose = goal_pose;
points.push_back(transform_pose(pose).pose);

// Plan route.
Expand All @@ -215,14 +243,34 @@ LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty.");
}
route.header.stamp = req->header.stamp;
route.header.stamp = header.stamp;
route.header.frame_id = map_frame_;
route.uuid.uuid = generate_random_id();
route.allow_modification = req->option.allow_goal_modification;
route.allow_modification = allow_goal_modification;

return route;
}

LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req)
{
const auto & header = req->header;
const auto & route_segments = req->segments;
const auto & goal_pose = req->goal;
const auto & allow_goal_modification = req->option.allow_goal_modification;

return create_route(header, route_segments, goal_pose, allow_goal_modification);
}

LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req)
{
const auto & header = req->header;
const auto & waypoints = req->waypoints;
const auto & goal_pose = req->goal;
const auto & allow_goal_modification = req->option.allow_goal_modification;

return create_route(header, waypoints, goal_pose, allow_goal_modification);
}

void MissionPlanner::change_state(RouteState::Message::_state_type state)
{
state_.stamp = now();
Expand Down Expand Up @@ -253,6 +301,11 @@ void MissionPlanner::on_set_route(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (is_emergency_) {
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot set route because of the emergency state");
return;
}

// Convert request to a new route.
const auto route = create_route(req);
Expand Down Expand Up @@ -283,6 +336,11 @@ void MissionPlanner::on_set_route_points(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (is_emergency_) {
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot set route points because of the emergency state");
return;
}

// Plan route.
const auto route = create_route(req);
Expand All @@ -299,19 +357,105 @@ 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;
using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response;
change_state(RouteState::Message::CHANGING);

if (!planner_->ready()) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready.");
}
if (!odometry_) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (!normal_route_) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set.");
}

// Plan route.
const auto new_route = create_route(req);

// check route safety
if (checkRerouteSafety(*normal_route_, new_route)) {
// sucess to reroute
change_mrm_route(new_route);
change_state(RouteState::Message::SET);
res->status.success = true;
is_emergency_ = true;
} else {
// failed to reroute
change_route(*normal_route_);
change_state(RouteState::Message::SET);
res->status.success = false;
is_emergency_ = false;
}
}

// NOTE: The route interface should be mutually exclusive by callback group.
void MissionPlanner::on_clear_mrm_route(
const ClearMrmRoute::Service::Request::SharedPtr req,
[[maybe_unused]] const ClearMrmRoute::Service::Request::SharedPtr req,
const ClearMrmRoute::Service::Response::SharedPtr res)
{
// TODO(Yutaka Shimizu): reroute for MRM
(void)req;
(void)res;
using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response;
change_state(RouteState::Message::CHANGING);

if (!planner_->ready()) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready.");
}
if (!odometry_) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (!normal_route_) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set.");
}
if (!mrm_route_) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "Mrm route is not set.");
}

// check route safety
if (checkRerouteSafety(*mrm_route_, *normal_route_)) {
clear_mrm_route();
change_route(*normal_route_);
change_state(RouteState::Message::SET);
res->success = true;
is_emergency_ = false;
return;
}

// reroute with normal goal
const std::vector<geometry_msgs::msg::Pose> empty_waypoints;
const auto new_route = create_route(
odometry_->header, empty_waypoints, normal_route_->goal_pose,
normal_route_->allow_modification);

// check new route safety
if (new_route.segments.empty() || !checkRerouteSafety(*mrm_route_, new_route)) {
// failed to create a new route
RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "Reroute with normal goal failed.");
change_mrm_route(*mrm_route_);
change_route(*normal_route_);
res->success = false;
is_emergency_ = true;
} else {
clear_mrm_route();
change_route(new_route);
res->success = true;
is_emergency_ = false;
}

change_state(RouteState::Message::SET);
}

void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg)
Expand All @@ -333,6 +477,11 @@ void MissionPlanner::on_change_route(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (is_emergency_) {
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot reroute because of the emergency state");
return;
}

// set to changing state
change_state(RouteState::Message::CHANGING);
Expand Down Expand Up @@ -375,6 +524,11 @@ void MissionPlanner::on_change_route_points(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (is_emergency_) {
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot reroute because of the emergency state");
return;
}

change_state(RouteState::Message::CHANGING);

Expand Down
13 changes: 12 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 @@ -75,9 +75,18 @@ class MissionPlanner : public rclcpp::Node

rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
void clear_route();
void clear_mrm_route();
void change_route(const LaneletRoute & route);
void change_mrm_route(const LaneletRoute & route);
LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req);
LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req);
LaneletRoute create_route(
const std_msgs::msg::Header & header,
const std::vector<autoware_adapi_v1_msgs::msg::RouteSegment> & route_segments,
const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification);
LaneletRoute create_route(
const std_msgs::msg::Header & header, const std::vector<geometry_msgs::msg::Pose> & waypoints,
const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification);

RouteState::Message state_;
component_interface_utils::Publisher<RouteState>::SharedPtr pub_state_;
Expand Down Expand Up @@ -105,7 +114,7 @@ class MissionPlanner : public rclcpp::Node
const SetMrmRoute::Service::Request::SharedPtr req,
const SetMrmRoute::Service::Response::SharedPtr res);
void on_clear_mrm_route(
const ClearMrmRoute::Service::Request::SharedPtr req,
[[maybe_unused]] const ClearMrmRoute::Service::Request::SharedPtr req,
const ClearMrmRoute::Service::Response::SharedPtr res);

HADMapBin::ConstSharedPtr map_ptr_{nullptr};
Expand All @@ -124,8 +133,10 @@ class MissionPlanner : public rclcpp::Node
double minimum_reroute_length_{30.0};
bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route);

bool is_emergency_{false};
std::shared_ptr<LaneletRoute> original_route_{nullptr};
std::shared_ptr<LaneletRoute> normal_route_{nullptr};
std::shared_ptr<LaneletRoute> mrm_route_{nullptr};
};

} // namespace mission_planner
Expand Down

0 comments on commit 358d4a7

Please sign in to comment.