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 reroute #3821

Merged
merged 2 commits into from
May 26, 2023
Merged

feat(mission_planner): add mrm reroute #3821

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
96d22a6
feat(mission_planner): add mrm reroute
purewater0901 May 25, 2023
ec8961a
update
purewater0901 May 25, 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
198 changes: 176 additions & 22 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,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
Open in desktop
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