diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml index ec7545956e774..0885f48a6827d 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,19 +1,10 @@ - - - - - - - - - - - - - - + + + + + diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt index f5677f2681767..5f7e9666e03c0 100644 --- a/planning/mission_planner/CMakeLists.txt +++ b/planning/mission_planner/CMakeLists.txt @@ -14,13 +14,20 @@ rclcpp_components_register_node(goal_pose_visualizer_component ) ament_auto_add_library(${PROJECT_NAME}_component SHARED - src/mission_planner/mission_planner.cpp src/mission_planner/arrival_checker.cpp + src/mission_planner/service_utils.cpp + src/mission_planner/mission_planner.cpp + src/mission_planner/route_selector.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_component PLUGIN "mission_planner::MissionPlanner" - EXECUTABLE ${PROJECT_NAME} + EXECUTABLE mission_planner +) + +rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "mission_planner::RouteSelector" + EXECUTABLE route_selector ) ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED @@ -30,7 +37,7 @@ ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED pluginlib_export_plugin_description_file(mission_planner plugins/plugin_description.xml) ament_auto_package( - INSTALL_TO_SHARE - config - launch + INSTALL_TO_SHARE + config + launch ) diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp deleted file mode 100644 index ddd0bfa652f0e..0000000000000 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ -#define MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace mission_planner -{ - -struct SetMrmRoute -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "~/srv/set_mrm_route"; -}; - -struct ClearMrmRoute -{ - using Service = autoware_adapi_v1_msgs::srv::ClearRoute; - static constexpr char name[] = "~/srv/clear_mrm_route"; -}; - -struct ModifiedGoal -{ - using Message = autoware_planning_msgs::msg::PoseWithUuidStamped; - static constexpr char name[] = "input/modified_goal"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; -}; - -} // namespace mission_planner - -#endif // MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml index 013516a18f813..eca6d05bb9463 100644 --- a/planning/mission_planner/launch/mission_planner.launch.xml +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -4,10 +4,24 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 4fcf2661ad20e..c892122f02969 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -17,10 +17,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs + autoware_auto_mapping_msgs autoware_planning_msgs - component_interface_specs - component_interface_utils geometry_msgs lanelet2_extension motion_utils @@ -28,7 +26,6 @@ rclcpp rclcpp_components route_handler - std_srvs tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index e98084204ba78..3793987b4c131 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -150,7 +150,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_goal_footprint_marker_ = - node_->create_publisher("debug/goal_footprint", durable_qos); + node_->create_publisher("~/debug/goal_footprint", durable_qos); vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); @@ -164,7 +164,7 @@ void DefaultPlanner::initialize(rclcpp::Node * node) { initialize_common(node); map_subscriber_ = node_->create_subscription( - "input/vector_map", rclcpp::QoS{10}.transient_local(), + "~/input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index e0181065150fd..97b864e22bec6 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -14,59 +14,19 @@ #include "mission_planner.hpp" +#include "service_utils.hpp" + #include #include #include #include -#include -#include #include #include #include #include -#include -#include - -namespace -{ - -using autoware_adapi_v1_msgs::msg::RoutePrimitive; -using autoware_adapi_v1_msgs::msg::RouteSegment; -using autoware_planning_msgs::msg::LaneletPrimitive; -using autoware_planning_msgs::msg::LaneletSegment; - -LaneletPrimitive convert(const RoutePrimitive & in) -{ - LaneletPrimitive out; - out.id = in.id; - out.primitive_type = in.type; - return out; -} - -LaneletSegment convert(const RouteSegment & in) -{ - LaneletSegment out; - out.primitives.reserve(in.alternatives.size() + 1); - out.primitives.push_back(convert(in.preferred)); - for (const auto & primitive : in.alternatives) { - out.primitives.push_back(convert(primitive)); - } - out.preferred_primitive = convert(in.preferred); - return out; -} - -std::array generate_random_id() -{ - static std::independent_bits_engine engine(std::random_device{}()); - std::array id; - std::generate(id.begin(), id.end(), std::ref(engine)); - return id; -} - -} // namespace namespace mission_planner { @@ -78,11 +38,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) tf_buffer_(get_clock()), tf_listener_(tf_buffer_), odometry_(nullptr), - map_ptr_(nullptr), - reroute_availability_(nullptr), - normal_route_(nullptr), - mrm_route_(nullptr) + map_ptr_(nullptr) { + using std::placeholders::_1; + using std::placeholders::_2; + map_frame_ = declare_parameter("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); minimum_reroute_length_ = declare_parameter("minimum_reroute_length"); @@ -90,76 +50,76 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - sub_odometry_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS(1), - std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); - sub_reroute_availability_ = create_subscription( - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/" - "is_reroute_available", - rclcpp::QoS(1), - std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); + const auto reroute_availability = std::make_shared(); + reroute_availability->availability = false; + reroute_availability_ = reroute_availability; const auto durable_qos = rclcpp::QoS(1).transient_local(); + sub_odometry_ = create_subscription( + "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); sub_vector_map_ = create_subscription( - "input/vector_map", durable_qos, - std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); + "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); + sub_reroute_availability_ = create_subscription( + "~/input/reroute_availability", rclcpp::QoS(1), + std::bind(&MissionPlanner::on_reroute_availability, this, _1)); + pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); + + // NOTE: The route interface should be mutually exclusive by callback group. sub_modified_goal_ = create_subscription( - "input/modified_goal", durable_qos, - std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - - pub_marker_ = create_publisher("debug/route_marker", durable_qos); - - const auto adaptor = component_interface_utils::NodeAdaptor(this); - adaptor.init_pub(pub_state_); - adaptor.init_pub(pub_route_); - adaptor.init_pub(pub_normal_route_); - adaptor.init_pub(pub_mrm_route_); - 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_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); - adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); + "~/input/modified_goal", durable_qos, std::bind(&MissionPlanner::on_modified_goal, this, _1)); + srv_clear_route = create_service( + "~/clear_route", service_utils::handle_exception(&MissionPlanner::on_clear_route, this)); + srv_set_lanelet_route = create_service( + "~/set_lanelet_route", + service_utils::handle_exception(&MissionPlanner::on_set_lanelet_route, this)); + srv_set_waypoint_route = create_service( + "~/set_waypoint_route", + service_utils::handle_exception(&MissionPlanner::on_set_waypoint_route, this)); + pub_route_ = create_publisher("~/route", durable_qos); + pub_state_ = create_publisher("~/state", durable_qos); // Route state will be published when the node gets ready for route api after initialization, // otherwise the mission planner rejects the request for the API. - data_check_timer_ = create_wall_timer( - std::chrono::milliseconds(100), std::bind(&MissionPlanner::checkInitialization, this)); + const auto period = rclcpp::Rate(10).period(); + data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); logger_configure_ = std::make_unique(this); } -void MissionPlanner::checkInitialization() +void MissionPlanner::check_initialization() { + auto logger = get_logger(); + auto clock = *get_clock(); + if (!planner_->ready()) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting lanelet map... Route API is not ready."); + RCLCPP_INFO_THROTTLE(logger, clock, 5000, "waiting lanelet map... Route API is not ready."); return; } if (!odometry_) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting odometry... Route API is not ready."); + RCLCPP_INFO_THROTTLE(logger, clock, 5000, "waiting odometry... Route API is not ready."); return; } // All data is ready. Now API is available. - RCLCPP_DEBUG(get_logger(), "Route API is ready."); - change_state(RouteState::Message::UNSET); - data_check_timer_->cancel(); // stop timer callback + RCLCPP_DEBUG(logger, "Route API is ready."); + change_state(RouteState::UNSET); + + // Stop timer callback. + data_check_timer_->cancel(); + data_check_timer_ = nullptr; } void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) { odometry_ = msg; - // NOTE: Do not check in the changing state as goal may change. - if (state_.state == RouteState::Message::SET) { + // NOTE: Do not check in the other states as goal may change. + if (state_.state == RouteState::SET) { PoseStamped pose; pose.header = odometry_->header; pose.pose = odometry_->pose.pose; if (arrival_checker_.is_arrived(pose)) { - change_state(RouteState::Message::ARRIVED); + change_state(RouteState::ARRIVED); } } } @@ -174,557 +134,261 @@ void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) map_ptr_ = msg; } -PoseStamped MissionPlanner::transform_pose(const PoseStamped & input) +Pose MissionPlanner::transform_pose(const Pose & pose, const Header & header) { - PoseStamped output; geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::Pose result; try { - transform = tf_buffer_.lookupTransform(map_frame_, input.header.frame_id, tf2::TimePointZero); - tf2::doTransform(input, output, transform); - return output; + transform = tf_buffer_.lookupTransform(map_frame_, header.frame_id, tf2::TimePointZero); + tf2::doTransform(pose, result, transform); + return result; } catch (tf2::TransformException & error) { - throw component_interface_utils::TransformError(error.what()); - } -} - -void MissionPlanner::clear_route() -{ - arrival_checker_.set_goal(); - planner_->clearRoute(); - normal_route_ = nullptr; - // 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; - goal.header = route.header; - goal.pose = route.goal_pose; - goal.uuid = route.uuid; - - arrival_checker_.set_goal(goal); - pub_route_->publish(route); - pub_normal_route_->publish(route); - pub_marker_->publish(planner_->visualize(route)); - planner_->updateRoute(route); - - // update normal route - normal_route_ = std::make_shared(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(route); -} - -LaneletRoute MissionPlanner::create_route( - const std_msgs::msg::Header & header, - const std::vector & route_segments, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) -{ - 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_stamped).pose; - for (const auto & segment : route_segments) { - route.segments.push_back(convert(segment)); + throw service_utils::TransformError(error.what()); } - route.header.stamp = header.stamp; - route.header.frame_id = map_frame_; - route.uuid.uuid = generate_random_id(); - route.allow_modification = allow_goal_modification; - - return route; -} - -LaneletRoute MissionPlanner::create_route( - const std_msgs::msg::Header & header, const std::vector & waypoints, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) -{ - // Use temporary pose stamped for transform. - PoseStamped pose; - pose.header = header; - - // Convert route points. - PlannerPlugin::RoutePoints points; - points.push_back(odometry_->pose.pose); - for (const auto & waypoint : waypoints) { - pose.pose = waypoint; - points.push_back(transform_pose(pose).pose); - } - pose.pose = goal_pose; - points.push_back(transform_pose(pose).pose); - - // Plan route. - LaneletRoute route = planner_->plan(points); - route.header.stamp = header.stamp; - route.header.frame_id = map_frame_; - route.uuid.uuid = generate_random_id(); - 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) +void MissionPlanner::change_state(RouteState::_state_type state) { state_.stamp = now(); state_.state = state; pub_state_->publish(state_); } -// 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) -{ - clear_route(); - change_state(RouteState::Message::UNSET); - res->status.success = true; -} - -// 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) +void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + RCLCPP_INFO(get_logger(), "Received modified goal."); - if (state_.state != RouteState::Message::UNSET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_ROUTE_EXISTS, "The route is already set."); + if (state_.state != RouteState::SET) { + RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); + return; } if (!planner_->ready()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + RCLCPP_ERROR(get_logger(), "The planner is not ready."); + return; } if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); + return; + } + if (!current_route_) { + RCLCPP_ERROR(get_logger(), "The route has not set yet."); + return; } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + if (current_route_->uuid != msg->uuid) { + RCLCPP_ERROR(get_logger(), "Goal uuid is incorrect."); + return; } - // Convert request to a new route. - const auto route = create_route(req); + change_state(RouteState::REROUTING); + const auto route = create_route(*msg); - // Check planned routes if (route.segments.empty()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + cancel_route(); + change_state(RouteState::SET); + RCLCPP_ERROR(get_logger(), "The planned route is empty."); } - // Update route. change_route(route); - change_state(RouteState::Message::SET); + change_state(RouteState::SET); + RCLCPP_INFO(get_logger(), "Changed the route with the modified goal"); +} + +void MissionPlanner::on_clear_route( + const ClearRoute::Request::SharedPtr, const ClearRoute::Response::SharedPtr res) +{ + change_route(); + change_state(RouteState::UNSET); res->status.success = true; } -// 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) +void MissionPlanner::on_set_lanelet_route( + const SetLaneletRoute::Request::SharedPtr req, const SetLaneletRoute::Response::SharedPtr res) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + const auto is_reroute = state_.state == RouteState::SET; - if (state_.state != RouteState::Message::UNSET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_ROUTE_EXISTS, "The route is already set."); + if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } if (!planner_->ready()) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); } if (!odometry_) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + if (is_reroute && !reroute_availability_->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } - // Plan route. - const auto route = create_route(req); + change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); + const auto route = create_route(*req); - // Check planned routes if (route.segments.empty()) { - throw component_interface_utils::ServiceException( + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - // Update route. + if (is_reroute && !check_reroute_safety(*current_route_, route)) { + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); + } + change_route(route); - change_state(RouteState::Message::SET); + change_state(RouteState::SET); res->status.success = true; } -// 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) +void MissionPlanner::on_set_waypoint_route( + const SetWaypointRoute::Request::SharedPtr req, const SetWaypointRoute::Response::SharedPtr res) { using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + const auto is_reroute = state_.state == RouteState::SET; + if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); + } if (!planner_->ready()) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); } if (!odometry_) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (reroute_availability_ && !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( + if (is_reroute && !reroute_availability_->availability) { + throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } - const auto prev_state = state_.state; - change_state(RouteState::Message::CHANGING); - - // Plan route. - const auto new_route = create_route(req); - - if (new_route.segments.empty()) { - change_state(prev_state); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "Failed to plan a new route."); - } + change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); + const auto route = create_route(*req); - // check route safety - // step1. if in mrm state, check with mrm route - if (mrm_route_) { - if (check_reroute_safety(*mrm_route_, new_route)) { - // success to reroute - change_mrm_route(new_route); - res->status.success = true; - } else { - // failed to reroute - change_mrm_route(*mrm_route_); - res->status.success = false; - } - change_state(RouteState::Message::SET); - RCLCPP_INFO(get_logger(), "Route is successfully changed with the modified goal"); - return; + if (route.segments.empty()) { + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (!normal_route_) { - // if it does not set normal route, just use the new planned route - change_mrm_route(new_route); - change_state(RouteState::Message::SET); - res->status.success = true; - RCLCPP_INFO(get_logger(), "MRM route is successfully changed with the modified goal"); - return; + if (is_reroute && !check_reroute_safety(*current_route_, route)) { + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); } - // step2. if not in mrm state, check with normal route - if (check_reroute_safety(*normal_route_, new_route)) { - // success to reroute - change_mrm_route(new_route); - res->status.success = true; - } else { - // Failed to reroute - change_route(*normal_route_); - res->status.success = false; - } - change_state(RouteState::Message::SET); + change_route(route); + change_state(RouteState::SET); + res->status.success = true; } -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_clear_mrm_route( - const ClearMrmRoute::Service::Request::SharedPtr, - const ClearMrmRoute::Service::Response::SharedPtr res) +void MissionPlanner::change_route() { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; - - if (!planner_->ready()) { - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); - } - if (!mrm_route_) { - throw component_interface_utils::NoEffectWarning("MRM route is not set"); - } - if ( - state_.state == RouteState::Message::SET && reroute_availability_ && - !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, - "Cannot clear MRM route as the planner is not lane following before arriving at the goal."); - } - - change_state(RouteState::Message::CHANGING); - - if (!normal_route_) { - clear_mrm_route(); - change_state(RouteState::Message::UNSET); - res->status.success = true; - return; - } - - // check route safety - if (check_reroute_safety(*mrm_route_, *normal_route_)) { - clear_mrm_route(); - change_route(*normal_route_); - change_state(RouteState::Message::SET); - res->status.success = true; - return; - } + current_route_ = nullptr; + planner_->clearRoute(); + arrival_checker_.set_goal(); - // reroute with normal goal - const std::vector 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() || !check_reroute_safety(*mrm_route_, new_route)) { - // failed to create a new route - RCLCPP_ERROR(get_logger(), "Reroute with normal goal failed."); - change_mrm_route(*mrm_route_); - change_state(RouteState::Message::SET); - res->status.success = false; - } else { - clear_mrm_route(); - change_route(new_route); - change_state(RouteState::Message::SET); - res->status.success = true; - } + // TODO(Takagi, Isamu): publish an empty route here + // pub_route_->publish(); + // pub_marker_->publish(); } -void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) +void MissionPlanner::change_route(const LaneletRoute & route) { - RCLCPP_INFO(get_logger(), "Received modified goal."); - - if (state_.state != RouteState::Message::SET) { - RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); - return; - } - if (!planner_->ready()) { - RCLCPP_ERROR(get_logger(), "The planner is not ready."); - return; - } - if (!odometry_) { - RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); - return; - } - if (!normal_route_) { - RCLCPP_ERROR(get_logger(), "Normal route has not set yet."); - return; - } - - if (mrm_route_ && mrm_route_->uuid == msg->uuid) { - // set to changing state - change_state(RouteState::Message::CHANGING); - - const std::vector empty_waypoints; - auto new_route = - create_route(msg->header, empty_waypoints, msg->pose, mrm_route_->allow_modification); - // create_route generate new uuid, so set the original uuid again to keep that. - new_route.uuid = msg->uuid; - if (new_route.segments.empty()) { - change_mrm_route(*mrm_route_); - change_state(RouteState::Message::SET); - RCLCPP_ERROR(get_logger(), "The planned MRM route is empty."); - return; - } - - change_mrm_route(new_route); - change_state(RouteState::Message::SET); - RCLCPP_INFO(get_logger(), "Changed the MRM route with the modified goal"); - return; - } - - if (normal_route_->uuid == msg->uuid) { - // set to changing state - change_state(RouteState::Message::CHANGING); - - const std::vector empty_waypoints; - auto new_route = - create_route(msg->header, empty_waypoints, msg->pose, normal_route_->allow_modification); - // create_route generate new uuid, so set the original uuid again to keep that. - new_route.uuid = msg->uuid; - if (new_route.segments.empty()) { - change_route(*normal_route_); - change_state(RouteState::Message::SET); - RCLCPP_ERROR(get_logger(), "The planned route is empty."); - return; - } + PoseWithUuidStamped goal; + goal.header = route.header; + goal.pose = route.goal_pose; + goal.uuid = route.uuid; - change_route(new_route); - change_state(RouteState::Message::SET); - RCLCPP_INFO(get_logger(), "Changed the route with the modified goal"); - return; - } + current_route_ = std::make_shared(route); + planner_->updateRoute(route); + arrival_checker_.set_goal(goal); - RCLCPP_ERROR(get_logger(), "Goal uuid is incorrect."); + pub_route_->publish(route); + pub_marker_->publish(planner_->visualize(route)); } -void MissionPlanner::on_change_route( - const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res) +void MissionPlanner::cancel_route() { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; - - if (state_.state != RouteState::Message::SET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); - } - if (!planner_->ready()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); - } - if (!normal_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); - } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); - } - if (reroute_availability_ && !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + // Restore planner state that changes with create_route function. + if (current_route_) { + planner_->updateRoute(*current_route_); } +} - // set to changing state - change_state(RouteState::Message::CHANGING); +LaneletRoute MissionPlanner::create_route(const SetLaneletRoute::Request & req) +{ + const auto & header = req.header; + const auto & segments = req.segments; + const auto & goal_pose = req.goal_pose; + const auto & uuid = req.uuid; + const auto & allow_goal_modification = req.allow_modification; - // Convert request to a new route. - const auto new_route = create_route(req); + return create_route(header, segments, goal_pose, uuid, allow_goal_modification); +} - // Check planned routes - if (new_route.segments.empty()) { - change_route(*normal_route_); - change_state(RouteState::Message::SET); - res->status.success = false; - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); - } +LaneletRoute MissionPlanner::create_route(const SetWaypointRoute::Request & req) +{ + const auto & header = req.header; + const auto & waypoints = req.waypoints; + const auto & goal_pose = req.goal_pose; + const auto & uuid = req.uuid; + const auto & allow_goal_modification = req.allow_modification; - // check route safety - if (check_reroute_safety(*normal_route_, new_route)) { - // success to reroute - change_route(new_route); - res->status.success = true; - change_state(RouteState::Message::SET); - } else { - // failed to reroute - change_route(*normal_route_); - res->status.success = false; - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); - } + return create_route(header, waypoints, goal_pose, uuid, allow_goal_modification); } -// 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) +LaneletRoute MissionPlanner::create_route(const PoseWithUuidStamped & msg) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + const auto & header = msg.header; + const auto & goal_pose = msg.pose; + const auto & uuid = msg.uuid; + const auto & allow_goal_modification = current_route_->allow_modification; - if (state_.state != RouteState::Message::SET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); - } - if (!planner_->ready()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); - } - if (!normal_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); - } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); - } - if (reroute_availability_ && !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); - } - - change_state(RouteState::Message::CHANGING); + return create_route(header, std::vector(), goal_pose, uuid, allow_goal_modification); +} - // Plan route. - const auto new_route = create_route(req); +LaneletRoute MissionPlanner::create_route( + const Header & header, const std::vector & segments, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification) +{ + LaneletRoute route; + route.header.stamp = header.stamp; + route.header.frame_id = map_frame_; + route.start_pose = odometry_->pose.pose; + route.goal_pose = transform_pose(goal_pose, header); + route.segments = segments; + route.uuid = uuid; + route.allow_modification = allow_goal_modification; + return route; +} - // Check planned routes - if (new_route.segments.empty()) { - change_state(RouteState::Message::SET); - change_route(*normal_route_); - res->status.success = false; - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); +LaneletRoute MissionPlanner::create_route( + const Header & header, const std::vector & waypoints, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification) +{ + PlannerPlugin::RoutePoints points; + points.push_back(odometry_->pose.pose); + for (const auto & waypoint : waypoints) { + points.push_back(transform_pose(waypoint, header)); } + points.push_back(transform_pose(goal_pose, header)); - // check route safety - if (check_reroute_safety(*normal_route_, new_route)) { - // success to reroute - change_route(new_route); - res->status.success = true; - change_state(RouteState::Message::SET); - } else { - // failed to reroute - change_route(*normal_route_); - res->status.success = false; - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); - } + LaneletRoute route = planner_->plan(points); + route.header.stamp = header.stamp; + route.header.frame_id = map_frame_; + route.uuid = uuid; + route.allow_modification = allow_goal_modification; + return route; } bool MissionPlanner::check_reroute_safety( diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index ea44d2643e186..04c1de79d5b6d 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -16,19 +16,22 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include -#include #include #include #include #include +#include +#include +#include #include #include #include +#include +#include +#include +#include #include #include @@ -41,23 +44,22 @@ namespace mission_planner { -using PoseStamped = geometry_msgs::msg::PoseStamped; -using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped; -using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; -using LaneletPrimitive = autoware_planning_msgs::msg::LaneletPrimitive; -using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; -using MarkerArray = visualization_msgs::msg::MarkerArray; -using ClearRoute = planning_interface::ClearRoute; -using SetRoutePoints = planning_interface::SetRoutePoints; -using SetRoute = planning_interface::SetRoute; -using ChangeRoutePoints = planning_interface::ChangeRoutePoints; -using ChangeRoute = planning_interface::ChangeRoute; -using Route = planning_interface::Route; -using NormalRoute = planning_interface::NormalRoute; -using MrmRoute = planning_interface::MrmRoute; -using RouteState = planning_interface::RouteState; -using Odometry = nav_msgs::msg::Odometry; -using RerouteAvailability = tier4_planning_msgs::msg::RerouteAvailability; +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PoseWithUuidStamped; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; +using std_msgs::msg::Header; +using tier4_planning_msgs::msg::RerouteAvailability; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetLaneletRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; +using unique_identifier_msgs::msg::UUID; +using visualization_msgs::msg::MarkerArray; class MissionPlanner : public rclcpp::Node { @@ -68,86 +70,63 @@ class MissionPlanner : public rclcpp::Node ArrivalChecker arrival_checker_; pluginlib::ClassLoader plugin_loader_; std::shared_ptr planner_; + std::string map_frame_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - PoseStamped transform_pose(const PoseStamped & input); + Pose transform_pose(const Pose & pose, const Header & header); + + rclcpp::Service::SharedPtr srv_clear_route; + rclcpp::Service::SharedPtr srv_set_lanelet_route; + rclcpp::Service::SharedPtr srv_set_waypoint_route; + rclcpp::Publisher::SharedPtr pub_state_; + rclcpp::Publisher::SharedPtr pub_route_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; - rclcpp::Subscription::SharedPtr sub_modified_goal_; + rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; HADMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; + RouteState state_; + LaneletRoute::ConstSharedPtr current_route_; + void on_odometry(const Odometry::ConstSharedPtr msg); void on_map(const HADMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); - rclcpp::TimerBase::SharedPtr data_check_timer_; - void checkInitialization(); - - rclcpp::Publisher::SharedPtr pub_marker_; - void clear_route(); - void clear_mrm_route(); + void on_clear_route( + const ClearRoute::Request::SharedPtr req, const ClearRoute::Response::SharedPtr res); + void on_set_lanelet_route( + const SetLaneletRoute::Request::SharedPtr req, const SetLaneletRoute::Response::SharedPtr res); + void on_set_waypoint_route( + const SetWaypointRoute::Request::SharedPtr req, + const SetWaypointRoute::Response::SharedPtr res); + + void change_state(RouteState::_state_type state); + void change_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); + void cancel_route(); + LaneletRoute create_route(const SetLaneletRoute::Request & req); + LaneletRoute create_route(const SetWaypointRoute::Request & req); + LaneletRoute create_route(const PoseWithUuidStamped & msg); LaneletRoute create_route( - const std_msgs::msg::Header & header, - const std::vector & route_segments, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); + const Header & header, const std::vector & segments, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification); LaneletRoute create_route( - const std_msgs::msg::Header & header, const std::vector & waypoints, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); - - RouteState::Message state_; - component_interface_utils::Publisher::SharedPtr pub_state_; - component_interface_utils::Publisher::SharedPtr pub_route_; - component_interface_utils::Publisher::SharedPtr pub_normal_route_; - component_interface_utils::Publisher::SharedPtr pub_mrm_route_; - void change_state(RouteState::Message::_state_type state); - - component_interface_utils::Service::SharedPtr srv_clear_route_; - component_interface_utils::Service::SharedPtr srv_set_route_; - component_interface_utils::Service::SharedPtr srv_set_route_points_; - component_interface_utils::Service::SharedPtr srv_change_route_; - component_interface_utils::Service::SharedPtr srv_change_route_points_; - void on_clear_route( - const ClearRoute::Service::Request::SharedPtr req, - const ClearRoute::Service::Response::SharedPtr res); - void on_set_route( - const SetRoute::Service::Request::SharedPtr req, - const SetRoute::Service::Response::SharedPtr res); - void on_set_route_points( - const SetRoutePoints::Service::Request::SharedPtr req, - const SetRoutePoints::Service::Response::SharedPtr res); - - component_interface_utils::Service::SharedPtr srv_set_mrm_route_; - component_interface_utils::Service::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); - - 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); - - double reroute_time_threshold_{10.0}; - double minimum_reroute_length_{30.0}; - bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); + const Header & header, const std::vector & waypoints, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification); - std::shared_ptr normal_route_{nullptr}; - std::shared_ptr mrm_route_{nullptr}; + rclcpp::TimerBase::SharedPtr data_check_timer_; + void check_initialization(); + + double reroute_time_threshold_; + double minimum_reroute_length_; + bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::unique_ptr logger_configure_; }; diff --git a/planning/mission_planner/src/mission_planner/route_selector.cpp b/planning/mission_planner/src/mission_planner/route_selector.cpp new file mode 100644 index 0000000000000..7547333d55cd1 --- /dev/null +++ b/planning/mission_planner/src/mission_planner/route_selector.cpp @@ -0,0 +1,302 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "route_selector.hpp" + +#include "service_utils.hpp" + +#include +#include +#include + +namespace mission_planner::uuid +{ + +std::array generate_random_id() +{ + static std::independent_bits_engine engine(std::random_device{}()); + std::array id; + std::generate(id.begin(), id.end(), std::ref(engine)); + return id; +} + +UUID generate_if_empty(const UUID & uuid) +{ + constexpr std::array zero_uuid = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + UUID result; + result.uuid = (uuid.uuid == zero_uuid) ? generate_random_id() : uuid.uuid; + return result; +} + +} // namespace mission_planner::uuid + +namespace mission_planner +{ + +RouteInterface::RouteInterface(rclcpp::Clock::SharedPtr clock) +{ + clock_ = clock; + state_.state = RouteState::UNKNOWN; +} + +RouteState::_state_type RouteInterface::get_state() const +{ + return state_.state; +} + +std::optional RouteInterface::get_route() const +{ + return route_; +} + +void RouteInterface::change_route() +{ + route_ = std::nullopt; +} + +void RouteInterface::change_state(RouteState::_state_type state) +{ + state_.stamp = clock_->now(); + state_.state = state; + pub_state_->publish(state_); +} + +void RouteInterface::update_state(const RouteState & state) +{ + state_ = state; + pub_state_->publish(state_); +} + +void RouteInterface::update_route(const LaneletRoute & route) +{ + route_ = route; + pub_route_->publish(route); +} + +RouteSelector::RouteSelector(const rclcpp::NodeOptions & options) +: Node("route_selector", options), main_(get_clock()), mrm_(get_clock()) +{ + using std::placeholders::_1; + using std::placeholders::_2; + const auto service_qos = rmw_qos_profile_services_default; + const auto durable_qos = rclcpp::QoS(1).transient_local(); + + // Init main route interface. + main_.srv_clear_route = create_service( + "~/main/clear_route", + service_utils::handle_exception(&RouteSelector::on_clear_route_main, this)); + main_.srv_set_waypoint_route = create_service( + "~/main/set_waypoint_route", + service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_main, this)); + main_.srv_set_lanelet_route = create_service( + "~/main/set_lanelet_route", + service_utils::handle_exception(&RouteSelector::on_set_lanelet_route_main, this)); + main_.pub_state_ = create_publisher("~/main/state", durable_qos); + main_.pub_route_ = create_publisher("~/main/route", durable_qos); + + // Init mrm route interface. + mrm_.srv_clear_route = create_service( + "~/mrm/clear_route", service_utils::handle_exception(&RouteSelector::on_clear_route_mrm, this)); + mrm_.srv_set_waypoint_route = create_service( + "~/mrm/set_waypoint_route", + service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_mrm, this)); + mrm_.srv_set_lanelet_route = create_service( + "~/mrm/set_lanelet_route", + service_utils::handle_exception(&RouteSelector::on_set_lanelet_route_mrm, this)); + mrm_.pub_state_ = create_publisher("~/mrm/state", durable_qos); + mrm_.pub_route_ = create_publisher("~/mrm/route", durable_qos); + + // Init mission planner interface. + group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cli_clear_route_ = create_client("~/planner/clear_route", service_qos, group_); + cli_set_lanelet_route_ = + create_client("~/planner/set_lanelet_route", service_qos, group_); + cli_set_waypoint_route_ = + create_client("~/planner/set_waypoint_route", service_qos, group_); + sub_state_ = create_subscription( + "~/planner/state", durable_qos, std::bind(&RouteSelector::on_state, this, _1)); + sub_route_ = create_subscription( + "~/planner/route", durable_qos, std::bind(&RouteSelector::on_route, this, _1)); + + // Set initial state. + main_.change_state(RouteState::INITIALIZING); + mrm_.change_state(RouteState::INITIALIZING); + initialized_ = false; + mrm_operating_ = false; + main_request_ = std::monostate{}; +} + +void RouteSelector::on_state(const RouteState::ConstSharedPtr msg) +{ + if (msg->state == RouteState::UNSET && !initialized_) { + main_.change_state(RouteState::UNSET); + mrm_.change_state(RouteState::UNSET); + initialized_ = true; + } + + (mrm_operating_ ? mrm_ : main_).update_state(*msg); +} + +void RouteSelector::on_route(const LaneletRoute::ConstSharedPtr msg) +{ + (mrm_operating_ ? mrm_ : main_).update_route(*msg); +} + +void RouteSelector::on_clear_route_main( + ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res) +{ + // Save the request and clear old route to resume from MRM. + main_request_ = std::monostate{}; + main_.change_route(); + + // During MRM, only change the state. + if (mrm_operating_) { + main_.change_state(RouteState::UNSET); + res->status.success = true; + return; + } + + // Forward the request if not in MRM. + res->status = service_utils::sync_call(cli_clear_route_, req); +} + +void RouteSelector::on_set_waypoint_route_main( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res) +{ + // Save the request and clear old route to resume from MRM. + req->uuid = uuid::generate_if_empty(req->uuid); + main_request_ = req; + main_.change_route(); + + // During MRM, only change the state. + if (mrm_operating_) { + main_.change_state(RouteState::INTERRUPTED); + res->status.success = true; + return; + } + + // Forward the request if not in MRM. + res->status = service_utils::sync_call(cli_set_waypoint_route_, req); +} + +void RouteSelector::on_set_lanelet_route_main( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res) +{ + // Save the request and clear old route to resume from MRM. + req->uuid = uuid::generate_if_empty(req->uuid); + main_request_ = req; + main_.change_route(); + + // During MRM, only change the state. + if (mrm_operating_) { + main_.change_state(RouteState::INTERRUPTED); + res->status.success = true; + return; + } + + // Forward the request if not in MRM. + res->status = service_utils::sync_call(cli_set_lanelet_route_, req); +} + +void RouteSelector::on_clear_route_mrm( + ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res) +{ + res->status = resume_main_route(req); + + if (res->status.success) { + mrm_operating_ = false; + mrm_.change_state(RouteState::UNSET); + } +} + +void RouteSelector::on_set_waypoint_route_mrm( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res) +{ + req->uuid = uuid::generate_if_empty(req->uuid); + res->status = service_utils::sync_call(cli_set_waypoint_route_, req); + + if (res->status.success) { + mrm_operating_ = true; + if (main_.get_state() != RouteState::UNSET) { + main_.change_state(RouteState::INTERRUPTED); + } + } +} + +void RouteSelector::on_set_lanelet_route_mrm( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res) +{ + req->uuid = uuid::generate_if_empty(req->uuid); + res->status = service_utils::sync_call(cli_set_lanelet_route_, req); + + if (res->status.success) { + mrm_operating_ = true; + if (main_.get_state() != RouteState::UNSET) { + main_.change_state(RouteState::INTERRUPTED); + } + } +} + +ResponseStatus RouteSelector::resume_main_route(ClearRoute::Request::SharedPtr req) +{ + const auto create_lanelet_request = [](const LaneletRoute & route) { + // NOTE: The start_pose.is not included in the request. + const auto r = std::make_shared(); + r->header = route.header; + r->goal_pose = route.goal_pose; + r->segments = route.segments; + r->uuid = route.uuid; + r->allow_modification = route.allow_modification; + return r; + }; + + const auto create_goal_request = [](const auto & request) { + const auto r = std::make_shared(); + r->header = request->header; + r->goal_pose = request->goal_pose; + r->uuid = request->uuid; + r->allow_modification = request->allow_modification; + return r; + }; + + // Clear the route if there is no request for the main route. + if (std::holds_alternative(main_request_)) { + return service_utils::sync_call(cli_clear_route_, req); + } + + // Attempt to resume the main route if there is a planned route. + if (const auto route = main_.get_route()) { + const auto r = create_lanelet_request(route.value()); + const auto status = service_utils::sync_call(cli_set_lanelet_route_, r); + if (status.success) return status; + } + + // If resuming fails, replan main route using the goal. + // NOTE: Clear the waypoints to avoid returning. Remove this once resuming is supported. + if (const auto request = std::get_if(&main_request_)) { + const auto r = create_goal_request(*request); + return service_utils::sync_call(cli_set_waypoint_route_, r); + } + if (const auto request = std::get_if(&main_request_)) { + const auto r = create_goal_request(*request); + return service_utils::sync_call(cli_set_waypoint_route_, r); + } + throw std::logic_error("route_selector: unknown main route request"); +} + +} // namespace mission_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mission_planner::RouteSelector) diff --git a/planning/mission_planner/src/mission_planner/route_selector.hpp b/planning/mission_planner/src/mission_planner/route_selector.hpp new file mode 100644 index 0000000000000..3226d1a7cb993 --- /dev/null +++ b/planning/mission_planner/src/mission_planner/route_selector.hpp @@ -0,0 +1,106 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MISSION_PLANNER__ROUTE_SELECTOR_HPP_ +#define MISSION_PLANNER__ROUTE_SELECTOR_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace mission_planner +{ + +using autoware_common_msgs::msg::ResponseStatus; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetLaneletRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; +using unique_identifier_msgs::msg::UUID; + +class RouteInterface +{ +public: + explicit RouteInterface(rclcpp::Clock::SharedPtr clock); + RouteState::_state_type get_state() const; + std::optional get_route() const; + void change_route(); + void change_state(RouteState::_state_type state); + void update_state(const RouteState & state); + void update_route(const LaneletRoute & route); + + rclcpp::Service::SharedPtr srv_clear_route; + rclcpp::Service::SharedPtr srv_set_lanelet_route; + rclcpp::Service::SharedPtr srv_set_waypoint_route; + rclcpp::Publisher::SharedPtr pub_state_; + rclcpp::Publisher::SharedPtr pub_route_; + +private: + RouteState state_; + std::optional route_; + rclcpp::Clock::SharedPtr clock_; +}; + +class RouteSelector : public rclcpp::Node +{ +public: + explicit RouteSelector(const rclcpp::NodeOptions & options); + +private: + using WaypointRequest = SetWaypointRoute::Request::SharedPtr; + using LaneletRequest = SetLaneletRoute::Request::SharedPtr; + + RouteInterface main_; + RouteInterface mrm_; + + rclcpp::CallbackGroup::SharedPtr group_; + rclcpp::Client::SharedPtr cli_clear_route_; + rclcpp::Client::SharedPtr cli_set_waypoint_route_; + rclcpp::Client::SharedPtr cli_set_lanelet_route_; + rclcpp::Subscription::SharedPtr sub_state_; + rclcpp::Subscription::SharedPtr sub_route_; + + bool initialized_; + bool mrm_operating_; + std::variant main_request_; + + void on_state(const RouteState::ConstSharedPtr msg); + void on_route(const LaneletRoute::ConstSharedPtr msg); + + void on_clear_route_main(ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res); + void on_set_waypoint_route_main( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res); + void on_set_lanelet_route_main( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res); + + void on_clear_route_mrm(ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res); + void on_set_waypoint_route_mrm( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res); + void on_set_lanelet_route_mrm( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res); + + ResponseStatus resume_main_route(ClearRoute::Request::SharedPtr req); +}; + +} // namespace mission_planner + +#endif // MISSION_PLANNER__ROUTE_SELECTOR_HPP_ diff --git a/planning/mission_planner/src/mission_planner/service_utils.cpp b/planning/mission_planner/src/mission_planner/service_utils.cpp new file mode 100644 index 0000000000000..9b3fc77424d5b --- /dev/null +++ b/planning/mission_planner/src/mission_planner/service_utils.cpp @@ -0,0 +1,32 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "service_utils.hpp" + +#include + +namespace service_utils +{ + +ServiceException ServiceUnready(const std::string & message) +{ + return ServiceException(ResponseStatus::SERVICE_UNREADY, message, false); +} + +ServiceException TransformError(const std::string & message) +{ + return ServiceException(ResponseStatus::TRANSFORM_ERROR, message, false); +}; + +} // namespace service_utils diff --git a/planning/mission_planner/src/mission_planner/service_utils.hpp b/planning/mission_planner/src/mission_planner/service_utils.hpp new file mode 100644 index 0000000000000..6e942ead9b383 --- /dev/null +++ b/planning/mission_planner/src/mission_planner/service_utils.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MISSION_PLANNER__SERVICE_UTILS_HPP_ +#define MISSION_PLANNER__SERVICE_UTILS_HPP_ + +#include + +#include +#include + +namespace service_utils +{ + +using ResponseStatus = autoware_common_msgs::msg::ResponseStatus; +using ResponseStatusCode = ResponseStatus::_code_type; + +class ServiceException : public std::exception +{ +public: + ServiceException(ResponseStatusCode code, const std::string & message, bool success = false) + { + success_ = success; + code_ = code; + message_ = message; + } + + template + void set(StatusT & status) const + { + status.success = success_; + status.code = code_; + status.message = message_; + } + +private: + bool success_; + ResponseStatusCode code_; + std::string message_; +}; + +ServiceException ServiceUnready(const std::string & message); +ServiceException TransformError(const std::string & message); + +template +std::function handle_exception(void (T::*callback)(Req, Res), T * instance) +{ + return [instance, callback](Req req, Res res) { + try { + (instance->*callback)(req, res); + } catch (const ServiceException & error) { + error.set(res->status); + } + }; +} + +template +ResponseStatus sync_call(T & client, Req req) +{ + if (!client->service_is_ready()) { + throw ServiceUnready(client->get_service_name()); + } + auto future = client->async_send_request(req); + return future.get()->status; +} + +} // namespace service_utils + +#endif // MISSION_PLANNER__SERVICE_UTILS_HPP_