diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index 435d5535e0a37..f966c3ca6da5e 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -4,6 +4,10 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(${PROJECT_NAME} SHARED + src/service_utils.cpp +) + include_directories( include SYSTEM diff --git a/planning/mission_planner/src/mission_planner/service_utils.hpp b/common/component_interface_utils/include/component_interface_utils/service_utils.hpp similarity index 89% rename from planning/mission_planner/src/mission_planner/service_utils.hpp rename to common/component_interface_utils/include/component_interface_utils/service_utils.hpp index 6e942ead9b383..aeac07032a02b 100644 --- a/planning/mission_planner/src/mission_planner/service_utils.hpp +++ b/common/component_interface_utils/include/component_interface_utils/service_utils.hpp @@ -12,15 +12,15 @@ // 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_ +#ifndef COMPONENT_INTERFACE_UTILS__SERVICE_UTILS_HPP_ +#define COMPONENT_INTERFACE_UTILS__SERVICE_UTILS_HPP_ #include #include #include -namespace service_utils +namespace component_interface_utils { using ResponseStatus = autoware_common_msgs::msg::ResponseStatus; @@ -75,6 +75,6 @@ ResponseStatus sync_call(T & client, Req req) return future.get()->status; } -} // namespace service_utils +} // namespace component_interface_utils -#endif // MISSION_PLANNER__SERVICE_UTILS_HPP_ +#endif // COMPONENT_INTERFACE_UTILS__SERVICE_UTILS_HPP_ diff --git a/planning/mission_planner/src/mission_planner/service_utils.cpp b/common/component_interface_utils/src/service_utils.cpp similarity index 87% rename from planning/mission_planner/src/mission_planner/service_utils.cpp rename to common/component_interface_utils/src/service_utils.cpp index 9b3fc77424d5b..fe54533030c6e 100644 --- a/planning/mission_planner/src/mission_planner/service_utils.cpp +++ b/common/component_interface_utils/src/service_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "service_utils.hpp" +#include "component_interface_utils/service_utils.hpp" #include -namespace service_utils +namespace component_interface_utils { ServiceException ServiceUnready(const std::string & message) @@ -29,4 +29,4 @@ ServiceException TransformError(const std::string & message) return ServiceException(ResponseStatus::TRANSFORM_ERROR, message, false); }; -} // namespace service_utils +} // namespace component_interface_utils diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt index 5f7e9666e03c0..4abb52b85d2bd 100644 --- a/planning/mission_planner/CMakeLists.txt +++ b/planning/mission_planner/CMakeLists.txt @@ -15,7 +15,6 @@ rclcpp_components_register_node(goal_pose_visualizer_component ament_auto_add_library(${PROJECT_NAME}_component SHARED src/mission_planner/arrival_checker.cpp - src/mission_planner/service_utils.cpp src/mission_planner/mission_planner.cpp src/mission_planner/route_selector.cpp ) diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index b8ad634c9a0d4..0297dab9fbc2f 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_planning_msgs + component_interface_utils geometry_msgs lanelet2_extension motion_utils diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index af363bcf23da7..40e71486e178e 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -14,8 +14,7 @@ #include "mission_planner.hpp" -#include "service_utils.hpp" - +#include #include #include #include @@ -69,13 +68,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) sub_modified_goal_ = create_subscription( "~/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)); + "~/clear_route", + component_interface_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)); + component_interface_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)); + component_interface_utils::handle_exception(&MissionPlanner::on_set_waypoint_route, this)); pub_route_ = create_publisher("~/route", durable_qos); pub_state_ = create_publisher("~/state", durable_qos); @@ -157,7 +157,7 @@ Pose MissionPlanner::transform_pose(const Pose & pose, const Header & header) tf2::doTransform(pose, result, transform); return result; } catch (tf2::TransformException & error) { - throw service_utils::TransformError(error.what()); + throw component_interface_utils::TransformError(error.what()); } } @@ -222,19 +222,19 @@ void MissionPlanner::on_set_lanelet_route( const auto is_reroute = state_.state == RouteState::SET; if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } if (!planner_->ready()) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); } if (!odometry_) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } if (is_reroute && !reroute_availability_->availability) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } @@ -244,14 +244,14 @@ void MissionPlanner::on_set_lanelet_route( if (route.segments.empty()) { cancel_route(); change_state(is_reroute ? RouteState::SET : RouteState::UNSET); - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } if (is_reroute && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); } @@ -270,19 +270,19 @@ void MissionPlanner::on_set_waypoint_route( const auto is_reroute = state_.state == RouteState::SET; if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } if (!planner_->ready()) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); } if (!odometry_) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } if (is_reroute && !reroute_availability_->availability) { - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } @@ -292,14 +292,14 @@ void MissionPlanner::on_set_waypoint_route( if (route.segments.empty()) { cancel_route(); change_state(is_reroute ? RouteState::SET : RouteState::UNSET); - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } if (is_reroute && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); - throw service_utils::ServiceException( + throw component_interface_utils::ServiceException( ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); } diff --git a/planning/mission_planner/src/mission_planner/route_selector.cpp b/planning/mission_planner/src/mission_planner/route_selector.cpp index 7547333d55cd1..036db8315ac30 100644 --- a/planning/mission_planner/src/mission_planner/route_selector.cpp +++ b/planning/mission_planner/src/mission_planner/route_selector.cpp @@ -14,7 +14,7 @@ #include "route_selector.hpp" -#include "service_utils.hpp" +#include #include #include @@ -96,25 +96,26 @@ RouteSelector::RouteSelector(const rclcpp::NodeOptions & options) // Init main route interface. main_.srv_clear_route = create_service( "~/main/clear_route", - service_utils::handle_exception(&RouteSelector::on_clear_route_main, this)); + component_interface_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)); + component_interface_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)); + component_interface_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/clear_route", + component_interface_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)); + component_interface_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)); + component_interface_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); @@ -169,7 +170,7 @@ void RouteSelector::on_clear_route_main( } // Forward the request if not in MRM. - res->status = service_utils::sync_call(cli_clear_route_, req); + res->status = component_interface_utils::sync_call(cli_clear_route_, req); } void RouteSelector::on_set_waypoint_route_main( @@ -188,7 +189,7 @@ void RouteSelector::on_set_waypoint_route_main( } // Forward the request if not in MRM. - res->status = service_utils::sync_call(cli_set_waypoint_route_, req); + res->status = component_interface_utils::sync_call(cli_set_waypoint_route_, req); } void RouteSelector::on_set_lanelet_route_main( @@ -207,7 +208,7 @@ void RouteSelector::on_set_lanelet_route_main( } // Forward the request if not in MRM. - res->status = service_utils::sync_call(cli_set_lanelet_route_, req); + res->status = component_interface_utils::sync_call(cli_set_lanelet_route_, req); } void RouteSelector::on_clear_route_mrm( @@ -225,7 +226,7 @@ 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); + res->status = component_interface_utils::sync_call(cli_set_waypoint_route_, req); if (res->status.success) { mrm_operating_ = true; @@ -239,7 +240,7 @@ 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); + res->status = component_interface_utils::sync_call(cli_set_lanelet_route_, req); if (res->status.success) { mrm_operating_ = true; @@ -273,13 +274,13 @@ ResponseStatus RouteSelector::resume_main_route(ClearRoute::Request::SharedPtr 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); + return component_interface_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); + const auto status = component_interface_utils::sync_call(cli_set_lanelet_route_, r); if (status.success) return status; } @@ -287,11 +288,11 @@ ResponseStatus RouteSelector::resume_main_route(ClearRoute::Request::SharedPtr r // 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); + return component_interface_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); + return component_interface_utils::sync_call(cli_set_waypoint_route_, r); } throw std::logic_error("route_selector: unknown main route request"); }