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 mrm route planner #4027

Merged
merged 7 commits into from
Jun 23, 2023
Merged

feat(mission_planner): add mrm route planner #4027

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7953ecb
feat(mission_planner): add mrm route planner
purewater0901 Jun 20, 2023
f67aa65
Merge branch 'main' into feat/mrm-route-planner
purewater0901 Jun 23, 2023
44e97b7
update
purewater0901 Jun 23, 2023
41357dc
update
purewater0901 Jun 23, 2023
df054e1
update
purewater0901 Jun 23, 2023
698cffa
update
purewater0901 Jun 23, 2023
d16313c
update
purewater0901 Jun 23, 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
147 changes: 139 additions & 8 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 @@ -75,7 +75,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"),
tf_buffer_(get_clock()),
tf_listener_(tf_buffer_),
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 @@ -155,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 @@ -172,6 +178,23 @@ void MissionPlanner::change_route(const LaneletRoute & route)
normal_route_ = std::make_shared<LaneletRoute>(route);
}

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_mrm_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,
Expand Down Expand Up @@ -278,6 +301,11 @@ void MissionPlanner::on_set_route(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (mrm_route_) {
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot set route in the emergency state");
return;
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
}

// Convert request to a new route.
const auto route = create_route(req);
Expand Down Expand Up @@ -313,6 +341,11 @@ void MissionPlanner::on_set_route_points(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received.");
}
if (mrm_route_) {
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot set route in the emergency state");
return;
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
}

// Plan route.
const auto route = create_route(req);
Expand All @@ -334,19 +367,107 @@ 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.");
}
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved

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

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

// Failed to reroute
if (mrm_route_) {
// if it has the old mrm route, use it
change_mrm_route(*mrm_route_);
} else {
// failed to go to mrm state, use normal route
change_route(*normal_route_);
}
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
change_state(RouteState::Message::SET);
res->status.success = 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,
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
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);
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved

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.");
}
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
if (!mrm_route_) {
change_state(RouteState::Message::SET);
throw component_interface_utils::ServiceException(
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
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;
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_);
change_state(RouteState::Message::SET);
res->success = false;
} else {
clear_mrm_route();
change_route(new_route);
change_state(RouteState::Message::SET);
res->success = true;
}
}

void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg)
Expand All @@ -367,6 +488,11 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt
RCLCPP_ERROR(get_logger(), "Normal route has not set yet.");
return;
}
if (mrm_route_) {
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot modify goal in the emergency state");
return;
}

if (normal_route_->uuid == msg->uuid) {
// set to changing state
Expand Down Expand Up @@ -468,6 +594,11 @@ void MissionPlanner::on_change_route_points(
throw component_interface_utils::ServiceException(
ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set.");
}
if (mrm_route_) {
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
RCLCPP_ERROR_THROTTLE(
get_logger(), *get_clock(), 5000, "Cannot reroute in the emergency state");
return;
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
}

change_state(RouteState::Message::CHANGING);

Expand Down
5 changes: 4 additions & 1 deletion 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 @@ -77,7 +77,9 @@ 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(
Expand Down Expand Up @@ -116,7 +118,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 @@ -136,6 +138,7 @@ class MissionPlanner : public rclcpp::Node
bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route);

std::shared_ptr<LaneletRoute> normal_route_{nullptr};
std::shared_ptr<LaneletRoute> mrm_route_{nullptr};
};

} // namespace mission_planner
Expand Down