Merge pull request #1611 from tier4/feature/set_route

Author: HansRobo

external/concealer/include/concealer/field_operator_application.hpp

@@ -21,6 +21,7 @@
 #include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
@@ -83,6 +84,7 @@ struct FieldOperatorApplication : public rclcpp::Node
   using CooperateCommands               = tier4_rtc_msgs::srv::CooperateCommands;
   using Engage                          = tier4_external_api_msgs::srv::Engage;
   using InitializeLocalization          = autoware_adapi_v1_msgs::srv::InitializeLocalization;
+  using SetRoute                        = autoware_adapi_v1_msgs::srv::SetRoute;
   using SetRoutePoints                  = autoware_adapi_v1_msgs::srv::SetRoutePoints;
   using AutoModeWithModule              = tier4_rtc_msgs::srv::AutoModeWithModule;
   using SetVelocityLimit                = tier4_external_api_msgs::srv::SetVelocityLimit;
@@ -109,6 +111,7 @@ struct FieldOperatorApplication : public rclcpp::Node
   Service<CooperateCommands>      requestCooperateCommands;
   Service<Engage>                 requestEngage;
   Service<InitializeLocalization> requestInitialPose;
+  Service<SetRoute>               requestSetRoute;
   Service<SetRoutePoints>         requestSetRoutePoints;
   Service<AutoModeWithModule>     requestSetRtcAutoMode;
   Service<SetVelocityLimit>       requestSetVelocityLimit;
@@ -159,7 +162,24 @@ struct FieldOperatorApplication : public rclcpp::Node
 
   auto initialize(const geometry_msgs::msg::Pose &) -> void;
 
-  auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &, const bool) -> void;
+  [[deprecated(
+    "This function was deprecated since version 16.5.0 (released on 20250603). It will be deleted "
+    "after a half-year transition period (~20251203). Please use other overloads instead.")]] auto
+  plan(const std::vector<geometry_msgs::msg::PoseStamped> &, const bool) -> void;
+
+#if __has_include(<autoware_adapi_v1_msgs/msg/route_option.hpp>)
+  using RouteOption = autoware_adapi_v1_msgs::msg::RouteOption;
+#else
+  using RouteOption = void;
+#endif
+
+  auto plan(
+    const geometry_msgs::msg::Pose & goal, const std::vector<geometry_msgs::msg::Pose> &,
+    const RouteOption &) -> void;
+
+  auto plan(
+    const geometry_msgs::msg::Pose & goal,
+    const std::vector<autoware_adapi_v1_msgs::msg::RouteSegment> &, const RouteOption &) -> void;
 
   auto clearRoute() -> void;
 

external/concealer/src/field_operator_application.cpp

@@ -156,7 +156,8 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid)
   requestCooperateCommands("/api/external/set/rtc_commands", *this),
   requestEngage("/api/external/set/engage", *this),
   requestInitialPose("/api/localization/initialize", *this),
-  // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons.
+  // NOTE: routing takes a long time to return. But the specified duration is not decided by any technical reasons.
+  requestSetRoute("/api/routing/set_route", *this, std::chrono::seconds(10)),
   requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)),
   requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this),
   requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this),
@@ -357,7 +358,67 @@ auto FieldOperatorApplication::plan(
 {
   assert(not route.empty());
 
-  task_queue.delay([this, route, allow_goal_modification]() {
+  std::vector<geometry_msgs::msg::Pose> waypoints;
+  if (route.size() > 1) {
+    std::transform(
+      route.begin(), route.end() - 1, std::back_inserter(waypoints),
+      [](const auto & stamped_pose) { return stamped_pose.pose; });
+  }
+
+  RouteOption route_option;
+  if (DetectMember_allow_goal_modification<RouteOption>::value) {
+    route_option.allow_goal_modification = allow_goal_modification;
+  }
+
+  plan(route.back().pose, waypoints, route_option);
+}
+
+template <
+  typename Request, typename Waypoint,
+  typename = std::enable_if_t<
+    std::is_same_v<Request, autoware_adapi_v1_msgs::srv::SetRoutePoints::Request> ||
+    std::is_same_v<Request, autoware_adapi_v1_msgs::srv::SetRoute::Request> > >
+static auto make(
+  const geometry_msgs::msg::Pose & goal, const std::vector<Waypoint> & waypoints,
+  const FieldOperatorApplication::RouteOption & option) -> std::shared_ptr<Request>
+{
+  auto request = std::make_shared<Request>();
+
+  request->header.frame_id = "map";
+  request->goal = goal;
+
+  if constexpr (std::is_same_v<Request, autoware_adapi_v1_msgs::srv::SetRoutePoints::Request>) {
+    request->waypoints.assign(waypoints.begin(), waypoints.end());
+  } else if constexpr (std::is_same_v<Request, autoware_adapi_v1_msgs::srv::SetRoute::Request>) {
+    request->segments.assign(waypoints.begin(), waypoints.end());
+  }
+
+  /*
+     NOTE: The autoware_adapi_v1_msgs::srv::SetRoutePoints::Request
+     type was created on 2022/09/05 [1], and the
+     autoware_adapi_v1_msgs::msg::Option type data member was added
+     to the autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type
+     on 2023/04/12 [2]. Therefore, we cannot expect
+     autoware_adapi_v1_msgs::srv::SetRoutePoints::Request to always
+     have a data member `option`.
+
+     [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef
+     [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419
+  */
+  if constexpr (
+    DetectMember_option<Request>::value and
+    DetectMember_allow_goal_modification<decltype(std::declval<Request>().option)>::value) {
+    request->option.allow_goal_modification = option.allow_goal_modification;
+  }
+
+  return request;
+}
+
+auto FieldOperatorApplication::plan(
+  const geometry_msgs::msg::Pose & goal, const std::vector<geometry_msgs::msg::Pose> & waypoints,
+  const RouteOption & option) -> void
+{
+  task_queue.delay([this, goal, waypoints, option]() {
     switch (const auto state = getLegacyAutowareState(); state.value) {
       default:
         throw common::AutowareError(
@@ -370,39 +431,35 @@ auto FieldOperatorApplication::plan(
         waitForAutowareStateToBe(state, LegacyAutowareState::waiting_for_route);
         [[fallthrough]];
       case LegacyAutowareState::waiting_for_route:
-        requestSetRoutePoints(
-          [&]() {
-            auto request = std::make_shared<SetRoutePoints::Request>();
-
-            request->header = route.back().header;
-            request->goal = route.back().pose;
-
-            for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) {
-              request->waypoints.push_back(each.pose);
-            }
-
-            /*
-               NOTE: The autoware_adapi_v1_msgs::srv::SetRoutePoints::Request
-               type was created on 2022/09/05 [1], and the
-               autoware_adapi_v1_msgs::msg::Option type data member was added
-               to the autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type
-               on 2023/04/12 [2]. Therefore, we cannot expect
-               autoware_adapi_v1_msgs::srv::SetRoutePoints::Request to always
-               have a data member `option`.
-
-               [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef
-               [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419
-            */
-            if constexpr (
-              DetectMember_option<SetRoutePoints::Request>::value and
-              DetectMember_allow_goal_modification<
-                decltype(std::declval<SetRoutePoints::Request>().option)>::value) {
-              request->option.allow_goal_modification = allow_goal_modification;
-            }
+        requestSetRoutePoints(make<SetRoutePoints::Request>(goal, waypoints, option), 30);
+        waitForAutowareStateToBe(
+          LegacyAutowareState::waiting_for_route, LegacyAutowareState::planning);
+        waitForAutowareStateToBe(
+          LegacyAutowareState::planning, LegacyAutowareState::waiting_for_engage);
+        break;
+    }
+  });
+}
 
-            return request;
-          }(),
-          30);
+auto FieldOperatorApplication::plan(
+  const geometry_msgs::msg::Pose & goal,
+  const std::vector<autoware_adapi_v1_msgs::msg::RouteSegment> & waypoints,
+  const RouteOption & option) -> void
+{
+  task_queue.delay([this, goal, waypoints, option]() {
+    switch (const auto state = getLegacyAutowareState(); state.value) {
+      default:
+        throw common::AutowareError(
+          "The simulator attempted to send a goal to Autoware, but was aborted because Autoware's "
+          "current state is ",
+          state, ".");
+      case LegacyAutowareState::initializing:
+        // The initial pose has been sent but has not yet reached Autoware.
+      case LegacyAutowareState::arrived_goal:
+        waitForAutowareStateToBe(state, LegacyAutowareState::waiting_for_route);
+        [[fallthrough]];
+      case LegacyAutowareState::waiting_for_route:
+        requestSetRoute(make<SetRoute::Request>(goal, waypoints, option), 30);
         waitForAutowareStateToBe(
           LegacyAutowareState::waiting_for_route, LegacyAutowareState::planning);
         waitForAutowareStateToBe(

openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp

@@ -53,45 +53,43 @@ auto AcquirePositionAction::start() -> void
       return applyAcquirePositionAction(actor, static_cast<NativeLanePosition>(position), options);
     });
 
-  auto allow_goal_modification_from_parameter = [&]() -> bool {
-    try {
-      return ref<Boolean>(std::string("AcquirePositionAction.allow_goal_modification"));
-    } catch (const SyntaxError &) {
-      // default value of allow_goal_modification
-      return false;
-    }
-  }();
-
-  auto get_allow_goal_modification_from_property =
-    [this](const EntityRef & entity_ref) -> std::optional<bool> {
+  auto get_from_controller_property =
+    [this](const EntityRef & entity_ref, const std::string & property_name) -> std::optional<bool> {
     if (const auto & entity = global().entities->ref(entity_ref);
         entity.template is<ScenarioObject>()) {
       const auto & object_controller = entity.as<ScenarioObject>().object_controller;
       if (object_controller.isAutoware() && object_controller.is<Controller>()) {
         auto controller = object_controller.as<Controller>();
-        if (controller.properties.contains("allowGoalModification")) {
-          return controller.properties.template get<Boolean>("allowGoalModification");
+        if (controller.properties.contains(property_name)) {
+          return controller.properties.template get<Boolean>(property_name);
         }
       }
     }
     return std::nullopt;
   };
 
+  auto get_from_parameter =
+    [&](const std::string & parameter_name, const bool default_value) -> bool {
+    try {
+      return ref<Boolean>(parameter_name);
+    } catch (const SyntaxError &) {
+      return default_value;
+    }
+  };
+
   for (const auto & actor : actors) {
     actor.apply([&](const auto & object) {
-      traffic_simulator::RouteOption options;
-      options.allow_goal_modification = [&]() {
-        if (auto allow_goal_modification_from_property =
-              get_allow_goal_modification_from_property(object);
-            allow_goal_modification_from_property.has_value()) {
+      traffic_simulator::v2::RouteOption route_option;
+      route_option.allow_goal_modification = [&]() {
+        if (auto from_property = get_from_controller_property(object, "allowGoalModification")) {
           // property specification takes precedence
-          return allow_goal_modification_from_property.value();
+          return from_property.value();
         } else {
-          return allow_goal_modification_from_parameter;
+          return get_from_parameter("RoutingAction__allow_goal_modification", false);
         }
       }();
 
-      apply<void>(acquire_position, position, object, options);
+      apply<void>(acquire_position, position, object, route_option);
     });
   }
 }

openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp

@@ -16,8 +16,11 @@
 #include <openscenario_interpreter/simulator_core.hpp>
 #include <openscenario_interpreter/syntax/assign_route_action.hpp>
 #include <openscenario_interpreter/syntax/catalog_reference.hpp>
+#include <openscenario_interpreter/syntax/controller.hpp>
+#include <openscenario_interpreter/syntax/entities.hpp>
 #include <openscenario_interpreter/syntax/object_type.hpp>
 #include <openscenario_interpreter/syntax/route.hpp>
+#include <openscenario_interpreter/syntax/scenario_object.hpp>
 #include <vector>
 
 namespace openscenario_interpreter
@@ -53,10 +56,47 @@ auto AssignRouteAction::run() -> void {}
 
 auto AssignRouteAction::start() -> void
 {
+  auto get_from_controller_property =
+    [this](const EntityRef & entity_ref, const std::string & property_name) -> std::optional<bool> {
+    if (const auto & entity = global().entities->ref(entity_ref);
+        entity.template is<ScenarioObject>()) {
+      const auto & object_controller = entity.as<ScenarioObject>().object_controller;
+      if (object_controller.isAutoware() && object_controller.is<Controller>()) {
+        auto controller = object_controller.as<Controller>();
+        if (controller.properties.contains(property_name)) {
+          return controller.properties.template get<Boolean>(property_name);
+        }
+      }
+    }
+    return std::nullopt;
+  };
+
+  auto get_from_parameter =
+    [&](const std::string & parameter_name, const bool default_value) -> bool {
+    try {
+      return ref<Boolean>(parameter_name);
+    } catch (const SyntaxError &) {
+      return default_value;
+    }
+  };
+
+  traffic_simulator::v2::RouteOption route_option;
+  route_option.use_lane_ids_for_routing =
+    get_from_parameter("RoutingAction__use_lane_ids_for_routing", false);
+
   for (const auto & actor : actors) {
     actor.apply([&](const auto & object) {
+      route_option.allow_goal_modification = [&]() {
+        if (auto from_property = get_from_controller_property(object, "allowGoalModification")) {
+          // property specification takes precedence
+          return from_property.value();
+        } else {
+          return get_from_parameter("RoutingAction__allow_goal_modification", false);
+        }
+      }();
       applyAssignRouteAction(
-        object, static_cast<std::vector<NativeLanePosition>>(route.as<const Route>()));
+        object, static_cast<std::vector<NativeLanePosition>>(route.as<const Route>()),
+        route_option);
     });
   }
 }

simulation/traffic_simulator/include/traffic_simulator/data_type/route_option.hpp

@@ -19,11 +19,29 @@ namespace traffic_simulator
 {
 inline namespace route_option
 {
-inline namespace v1
+inline namespace v2
 {
 struct RouteOption
 {
   bool allow_goal_modification = false;
+  bool use_lane_ids_for_routing = false;
+};
+}  // namespace v2
+
+namespace v1
+{
+struct RouteOption
+{
+  bool allow_goal_modification = false;
+
+  // conversion without loss of information
+  operator v2::RouteOption() const
+  {
+    v2::RouteOption v2;
+    v2.allow_goal_modification = allow_goal_modification;
+    v2.use_lane_ids_for_routing = false;
+    return v2;
+  }
 };
 }  // namespace v1
 }  // namespace route_option

simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp

@@ -82,21 +82,23 @@ class EgoEntity : public VehicleEntity, private concealer::FieldOperatorApplicat
 
   void requestAcquirePosition(const CanonicalizedLaneletPose &) override;
 
-  void requestAcquirePosition(const CanonicalizedLaneletPose &, const RouteOption &) override;
+  void requestAcquirePosition(
+    const CanonicalizedLaneletPose &, const traffic_simulator::RouteOption &) override;
 
   void requestAcquirePosition(const geometry_msgs::msg::Pose &) override;
 
-  void requestAcquirePosition(const geometry_msgs::msg::Pose &, const RouteOption &) override;
+  void requestAcquirePosition(
+    const geometry_msgs::msg::Pose &, const traffic_simulator::RouteOption &) override;
 
   void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override;
 
   void requestAssignRoute(
-    const std::vector<CanonicalizedLaneletPose> &, const RouteOption &) override;
+    const std::vector<CanonicalizedLaneletPose> &, const traffic_simulator::RouteOption &) override;
 
   void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override;
 
   void requestAssignRoute(
-    const std::vector<geometry_msgs::msg::Pose> &, const RouteOption &) override;
+    const std::vector<geometry_msgs::msg::Pose> &, const traffic_simulator::RouteOption &) override;
 
   auto requestFollowTrajectory(
     const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;