feat: move response status handling utilily

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Author: isamu-takagi

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

planning/mission_planner/src/mission_planner/service_utils.hpp 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 <autoware_common_msgs/msg/response_status.hpp>
 
 #include <stdexcept>
 #include <string>
 
-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_

planning/mission_planner/src/mission_planner/service_utils.cpp 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 <string>
 
-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

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
 )

planning/mission_planner/package.xml

@@ -20,6 +20,7 @@
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>component_interface_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>motion_utils</depend>

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -14,8 +14,7 @@
 
 #include "mission_planner.hpp"
 
-#include "service_utils.hpp"
-
+#include <component_interface_utils/service_utils.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/route_checker.hpp>
@@ -69,13 +68,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   sub_modified_goal_ = create_subscription<PoseWithUuidStamped>(
     "~/input/modified_goal", durable_qos, std::bind(&MissionPlanner::on_modified_goal, this, _1));
   srv_clear_route = create_service<ClearRoute>(
-    "~/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<SetLaneletRoute>(
     "~/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<SetWaypointRoute>(
     "~/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<LaneletRoute>("~/route", durable_qos);
   pub_state_ = create_publisher<RouteState>("~/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.");
   }
 

planning/mission_planner/src/mission_planner/route_selector.cpp

@@ -14,7 +14,7 @@
 
 #include "route_selector.hpp"
 
-#include "service_utils.hpp"
+#include <component_interface_utils/service_utils.hpp>
 
 #include <array>
 #include <memory>
@@ -96,25 +96,26 @@ RouteSelector::RouteSelector(const rclcpp::NodeOptions & options)
   // Init main route interface.
   main_.srv_clear_route = create_service<ClearRoute>(
     "~/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<SetWaypointRoute>(
     "~/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<SetLaneletRoute>(
     "~/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<RouteState>("~/main/state", durable_qos);
   main_.pub_route_ = create_publisher<LaneletRoute>("~/main/route", durable_qos);
 
   // Init mrm route interface.
   mrm_.srv_clear_route = create_service<ClearRoute>(
-    "~/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<SetWaypointRoute>(
     "~/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<SetLaneletRoute>(
     "~/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<RouteState>("~/mrm/state", durable_qos);
   mrm_.pub_route_ = create_publisher<LaneletRoute>("~/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,25 +274,25 @@ 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<std::monostate>(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;
   }
 
   // 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<WaypointRequest>(&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<LaneletRequest>(&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");
 }