feat(default_ad_api): apply route_selector

common/component_interface_specs/CMakeLists.txt

@@ -4,26 +4,8 @@ project(component_interface_specs)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-include_directories(
-  include
-  SYSTEM
-  ${rclcpp_INCLUDE_DIRS}
-  ${rosidl_runtime_cpp_INCLUDE_DIRS}
-  ${rcl_INCLUDE_DIRS}
-  ${autoware_adapi_v1_msgs_INCLUDE_DIRS}
-  ${autoware_auto_planning_msgs_INCLUDE_DIRS}
-  ${autoware_planning_msgs_INCLUDE_DIRS}
-  ${autoware_auto_vehicle_msgs_INCLUDE_DIRS}
-  ${tier4_control_msgs_INCLUDE_DIRS}
-  ${nav_msgs_INCLUDE_DIRS}
-  ${tier4_system_msgs_INCLUDE_DIRS}
-  ${tier4_vehicle_msgs_INCLUDE_DIRS}
-  ${autoware_auto_perception_msgs_INCLUDE_DIRS}
-  ${tier4_map_msgs_INCLUDE_DIRS}
-)
-
 if(BUILD_TESTING)
-  ament_add_ros_isolated_gtest(test_component_interface_specs
+  ament_auto_add_gtest(gtest_${PROJECT_NAME}
     test/gtest_main.cpp
     test/test_planning.cpp
     test/test_control.cpp
@@ -33,9 +15,6 @@ if(BUILD_TESTING)
     test/test_perception.cpp
     test/test_vehicle.cpp
   )
-  target_include_directories(test_component_interface_specs
-    PRIVATE include
-  )
 endif()
 
 ament_auto_package()

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -17,77 +17,48 @@
 
 #include <rclcpp/qos.hpp>
 
-#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
-#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
-#include <autoware_adapi_v1_msgs/srv/set_route.hpp>
-#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <tier4_planning_msgs/msg/route_state.hpp>
+#include <tier4_planning_msgs/srv/clear_route.hpp>
+#include <tier4_planning_msgs/srv/set_lanelet_route.hpp>
+#include <tier4_planning_msgs/srv/set_waypoint_route.hpp>
 
 namespace planning_interface
 {
 
-struct SetRoutePoints
+struct SetLaneletRoute
 {
-  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
-  static constexpr char name[] = "/planning/mission_planning/set_route_points";
+  using Service = tier4_planning_msgs::srv::SetLaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route";
 };
 
-struct SetRoute
+struct SetWaypointRoute
 {
-  using Service = autoware_adapi_v1_msgs::srv::SetRoute;
-  static constexpr char name[] = "/planning/mission_planning/set_route";
-};
-
-struct ChangeRoutePoints
-{
-  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
-  static constexpr char name[] = "/planning/mission_planning/change_route_points";
-};
-
-struct ChangeRoute
-{
-  using Service = autoware_adapi_v1_msgs::srv::SetRoute;
-  static constexpr char name[] = "/planning/mission_planning/change_route";
+  using Service = tier4_planning_msgs::srv::SetWaypointRoute;
+  static constexpr char name[] =
+    "/planning/mission_planning/route_selector/main/set_waypoint_route";
 };
 
 struct ClearRoute
 {
-  using Service = autoware_adapi_v1_msgs::srv::ClearRoute;
-  static constexpr char name[] = "/planning/mission_planning/clear_route";
+  using Service = tier4_planning_msgs::srv::ClearRoute;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route";
 };
 
 struct RouteState
 {
-  using Message = autoware_adapi_v1_msgs::msg::RouteState;
-  static constexpr char name[] = "/planning/mission_planning/route_state";
-  static constexpr size_t depth = 1;
-  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
-  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
-};
-
-struct Route
-{
-  using Message = autoware_planning_msgs::msg::LaneletRoute;
-  static constexpr char name[] = "/planning/mission_planning/route";
-  static constexpr size_t depth = 1;
-  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
-  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
-};
-
-struct NormalRoute
-{
-  using Message = autoware_planning_msgs::msg::LaneletRoute;
-  static constexpr char name[] = "/planning/mission_planning/normal_route";
+  using Message = tier4_planning_msgs::msg::RouteState;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/state";
   static constexpr size_t depth = 1;
   static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
-struct MrmRoute
+struct LaneletRoute
 {
   using Message = autoware_planning_msgs::msg::LaneletRoute;
-  static constexpr char name[] = "/planning/mission_planning/mrm_route";
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/route";
   static constexpr size_t depth = 1;
   static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;

common/component_interface_specs/package.xml

@@ -9,14 +9,8 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
-  <buildtool_depend>ament_cmake_core</buildtool_depend>
-  <buildtool_depend>ament_cmake_export_dependencies</buildtool_depend>
-  <buildtool_depend>ament_cmake_test</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
-  <buildtool_export_depend>ament_cmake_test</buildtool_export_depend>
-
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
@@ -28,9 +22,11 @@
   <depend>rosidl_runtime_cpp</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_map_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
   <depend>tier4_system_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>
-  <test_depend>ament_cmake_ros</test_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

common/component_interface_specs/test/test_planning.cpp

@@ -27,26 +27,8 @@ TEST(planning, interface)
   }
 
   {
-    using planning_interface::Route;
-    Route route;
-    size_t depth = 1;
-    EXPECT_EQ(route.depth, depth);
-    EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
-    EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
-  }
-
-  {
-    using planning_interface::NormalRoute;
-    NormalRoute route;
-    size_t depth = 1;
-    EXPECT_EQ(route.depth, depth);
-    EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
-    EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
-  }
-
-  {
-    using planning_interface::MrmRoute;
-    MrmRoute route;
+    using planning_interface::LaneletRoute;
+    LaneletRoute route;
     size_t depth = 1;
     EXPECT_EQ(route.depth, depth);
     EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);

system/default_ad_api/src/routing.cpp

@@ -18,6 +18,33 @@
 
 #include <memory>
 
+namespace
+{
+
+using autoware_adapi_v1_msgs::msg::ResponseStatus;
+
+template <class InterfaceT>
+ResponseStatus route_already_set()
+{
+  ResponseStatus status;
+  status.success = false;
+  status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE;
+  status.message = "The route is already set.";
+  return status;
+}
+
+template <class InterfaceT>
+ResponseStatus route_is_not_set()
+{
+  ResponseStatus status;
+  status.success = false;
+  status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE;
+  status.message = "The route is not set yet.";
+  return status;
+}
+
+}  // namespace
+
 namespace default_ad_api
 {
 
@@ -30,20 +57,25 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
   adaptor.init_sub(sub_state_, this, &RoutingNode::on_state);
   adaptor.init_sub(sub_route_, this, &RoutingNode::on_route);
   adaptor.init_cli(cli_clear_route_, group_cli_);
+  adaptor.init_cli(cli_set_waypoint_route_, group_cli_);
+  adaptor.init_cli(cli_set_lanelet_route_, group_cli_);
   adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route);
-  adaptor.relay_service(cli_set_route_, srv_set_route_, group_cli_);
-  adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_);
-  adaptor.relay_service(cli_change_route_, srv_change_route_, group_cli_);
-  adaptor.relay_service(cli_change_route_points_, srv_change_route_points_, group_cli_);
+  adaptor.init_srv(srv_set_route_, this, &RoutingNode::on_set_route);
+  adaptor.init_srv(srv_set_route_points_, this, &RoutingNode::on_set_route_points);
+  adaptor.init_srv(srv_change_route_, this, &RoutingNode::on_change_route);
+  adaptor.init_srv(srv_change_route_points_, this, &RoutingNode::on_change_route_points);
 
   adaptor.init_cli(cli_operation_mode_, group_cli_);
   adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode);
+
+  is_auto_mode_ = false;
+  state_.state = State::Message::UNKNOWN;
 }
 
 void RoutingNode::change_stop_mode()
 {
   using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request;
-  if (is_auto_mode) {
+  if (is_auto_mode_) {
     const auto req = std::make_shared<OperationModeRequest>();
     req->mode = OperationModeRequest::STOP;
     cli_operation_mode_->async_send_request(req);
@@ -52,12 +84,19 @@ void RoutingNode::change_stop_mode()
 
 void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg)
 {
-  is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS;
+  is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS;
 }
 
 void RoutingNode::on_state(const State::Message::ConstSharedPtr msg)
 {
-  pub_state_->publish(*msg);
+  // TODO(Takagi, Isamu): Add adapi initializing state.
+  // Represent initializing state by not publishing the topic for now.
+  if (msg->state == State::Message::INITIALIZING) {
+    return;
+  }
+
+  state_ = *msg;
+  pub_state_->publish(conversion::convert_state(*msg));
 
   // Change operation mode to stop when the vehicle arrives.
   if (msg->state == State::Message::ARRIVED) {
@@ -80,7 +119,51 @@ void RoutingNode::on_clear_route(
   const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res)
 {
   change_stop_mode();
-  *res = *cli_clear_route_->call(req);
+  res->status = conversion::convert_call(cli_clear_route_, req);
+}
+
+void RoutingNode::on_set_route_points(
+  const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
+  const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res)
+{
+  if (state_.state != State::Message::UNSET) {
+    res->status = route_already_set<autoware_ad_api::routing::SetRoutePoints>();
+    return;
+  }
+  res->status = conversion::convert_call(cli_set_waypoint_route_, req);
+}
+
+void RoutingNode::on_set_route(
+  const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
+  const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res)
+{
+  if (state_.state != State::Message::UNSET) {
+    res->status = route_already_set<autoware_ad_api::routing::SetRoute>();
+    return;
+  }
+  res->status = conversion::convert_call(cli_set_lanelet_route_, req);
+}
+
+void RoutingNode::on_change_route_points(
+  const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
+  const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res)
+{
+  if (state_.state != State::Message::SET) {
+    res->status = route_is_not_set<autoware_ad_api::routing::SetRoutePoints>();
+    return;
+  }
+  res->status = conversion::convert_call(cli_set_waypoint_route_, req);
+}
+
+void RoutingNode::on_change_route(
+  const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
+  const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res)
+{
+  if (state_.state != State::Message::SET) {
+    res->status = route_is_not_set<autoware_ad_api::routing::SetRoute>();
+    return;
+  }
+  res->status = conversion::convert_call(cli_set_lanelet_route_, req);
 }
 
 }  // namespace default_ad_api

system/default_ad_api/src/routing.hpp

@@ -33,6 +33,10 @@ class RoutingNode : public rclcpp::Node
   explicit RoutingNode(const rclcpp::NodeOptions & options);
 
 private:
+  using OperationModeState = system_interface::OperationModeState;
+  using State = planning_interface::RouteState;
+  using Route = planning_interface::LaneletRoute;
+
   rclcpp::CallbackGroup::SharedPtr group_cli_;
   Pub<autoware_ad_api::routing::RouteState> pub_state_;
   Pub<autoware_ad_api::routing::Route> pub_route_;
@@ -42,26 +46,34 @@ class RoutingNode : public rclcpp::Node
   Srv<autoware_ad_api::routing::ChangeRoute> srv_change_route_;
   Srv<autoware_ad_api::routing::ClearRoute> srv_clear_route_;
   Sub<planning_interface::RouteState> sub_state_;
-  Sub<planning_interface::Route> sub_route_;
-  Cli<planning_interface::SetRoutePoints> cli_set_route_points_;
-  Cli<planning_interface::SetRoute> cli_set_route_;
-  Cli<planning_interface::ChangeRoutePoints> cli_change_route_points_;
-  Cli<planning_interface::ChangeRoute> cli_change_route_;
+  Sub<planning_interface::LaneletRoute> sub_route_;
+  Cli<planning_interface::SetWaypointRoute> cli_set_waypoint_route_;
+  Cli<planning_interface::SetLaneletRoute> cli_set_lanelet_route_;
   Cli<planning_interface::ClearRoute> cli_clear_route_;
   Cli<system_interface::ChangeOperationMode> cli_operation_mode_;
   Sub<system_interface::OperationModeState> sub_operation_mode_;
-  bool is_auto_mode = false;
+  bool is_auto_mode_;
+  State::Message state_;
 
-  using OperationModeState = system_interface::OperationModeState;
-  using State = planning_interface::RouteState;
-  using Route = planning_interface::Route;
   void change_stop_mode();
   void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg);
   void on_state(const State::Message::ConstSharedPtr msg);
   void on_route(const Route::Message::ConstSharedPtr msg);
   void on_clear_route(
     const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req,
     const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res);
+  void on_set_route_points(
+    const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
+    const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res);
+  void on_set_route(
+    const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
+    const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res);
+  void on_change_route_points(
+    const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req,
+    const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res);
+  void on_change_route(
+    const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req,
+    const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res);
 };
 
 }  // namespace default_ad_api