Feature/manual on follow trajectory to merge

docs/developer_guide/.pages

@@ -18,3 +18,4 @@ nav:
     - CONTRIBUTING.md
     - Communication.md
     - ConfiguringPerceptionTopics.md
+    - ManualOverrideWithFollowTrajectoryAction.md

docs/developer_guide/ManualOverrideWithFollowTrajectoryAction.md

@@ -0,0 +1,40 @@
+# Manual Override Simulation with FollowTrajectoryAction
+
+`scenario_simulator_v2` simulates the manual override of Autoware, with `FollowTrajectoryAction`.
+During the executing `FollowTrajectoryAction`, the control of the ego entity is taken over from Autoware to the `FollowTrajectoryAction`.
+
+## 3 types of override for Autoware
+
+There are 3 types of override for Autoware.
+
+- Local: Manually control the vehicle from nearby with some device such as a joystick.
+  - This is one of operation modes.
+- Remote: Manually control the vehicle from a web application on the cloud.
+  - This is one of operation modes.
+- Direct: Manually control the vehicle from handle, brake and/or accel directly.
+  - Please note that this is not a operation mode but a control mode of vehicle interface.
+
+## override simulation in scenario_simulator_v2
+
+vehicle interface simulation is a part of the ego vehicle simulation feature in `scenario_simulator_v2`.
+`scenario_simulator_v2` simulates a `Direct` override triggered by safety operators when a scenario commands overriding the ego vehicle by `FollowTrajectoryAction`.
+
+## 3 steps scenario_simulator_v2 takes to simulate the overrides
+
+### 1. triggering the override
+
+In real vehicle, the override detected in vehicle internally and communicated to vehicle interface node such as `pacmod_interface` node.
+
+In `scenario_simulator_v2`, `openscenario_interpreter` send an override flag via zmq interface between `traffic_simulator` and `simple_sensor_simulator` when `FollowTrajectoryAction` is started.
+
+`simple_sensor_simulator` receives it and set the control mode to MANUAL like vehicle interface do when hardware override triggers detected.
+
+### 2. during the override
+
+`traffic_simulator` send ego status calculated to follow described in the scenario and `simple_sensor_simulator` overrides Autoware control with overwriting ego status by the received ego status.
+
+### 3. finishing the override
+
+When `FollowTrajectoryAction` is finished, `traffic_simulator` call service to enable autoware control and stop sending the override flag to `simple_sensor_simulator` via zmq communication.
+
+This mimics the steps safety operators do in real vehicle via some human interfaces, in API level.

external/concealer/include/concealer/autoware.hpp

@@ -17,6 +17,7 @@
 
 #include <atomic>
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <concealer/continuous_transform_broadcaster.hpp>
@@ -66,12 +67,19 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster<Auto
 
   virtual auto getRouteLanelets() const -> std::vector<std::int64_t> = 0;
 
+  virtual auto getControlModeReport() const
+    -> autoware_auto_vehicle_msgs::msg::ControlModeReport = 0;
+
   auto set(const geometry_msgs::msg::Accel &) -> void;
 
   auto set(const geometry_msgs::msg::Twist &) -> void;
 
   auto set(const geometry_msgs::msg::Pose &) -> void;
 
+  virtual auto setAutonomousMode() -> void = 0;
+
+  virtual auto setManualMode() -> void = 0;
+
   virtual auto rethrow() -> void;
 };
 }  // namespace concealer

external/concealer/include/concealer/autoware_universe.hpp

@@ -22,6 +22,7 @@
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <autoware_auto_vehicle_msgs/srv/control_mode_command.hpp>
 #include <concealer/autoware.hpp>
 #include <concealer/publisher_wrapper.hpp>
 #include <concealer/subscriber_wrapper.hpp>
@@ -37,20 +38,34 @@ namespace concealer
 class AutowareUniverse : public Autoware
 {
   // clang-format off
-  SubscriberWrapper<autoware_auto_control_msgs::msg::AckermannControlCommand, ThreadSafety::safe> getAckermannControlCommand;
-  SubscriberWrapper<autoware_auto_vehicle_msgs::msg::GearCommand,             ThreadSafety::safe> getGearCommandImpl;
-  SubscriberWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand,   ThreadSafety::safe> getTurnIndicatorsCommand;
-  SubscriberWrapper<autoware_auto_planning_msgs::msg::PathWithLaneId,         ThreadSafety::safe> getPathWithLaneId;
-
-  PublisherWrapper<geometry_msgs::msg::AccelWithCovarianceStamped>        setAcceleration;
-  PublisherWrapper<nav_msgs::msg::Odometry>                               setOdometry;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::SteeringReport>       setSteeringReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::GearReport>           setGearReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::ControlModeReport>    setControlModeReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::VelocityReport>       setVelocityReport;
-  PublisherWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport> setTurnIndicatorsReport;
+  using ControlModeCommand          = autoware_auto_vehicle_msgs::srv::ControlModeCommand;
+  using ControlModeReport           = autoware_auto_vehicle_msgs::msg::ControlModeReport;
+  using GearCommand                 = autoware_auto_vehicle_msgs::msg::GearCommand;
+  using GearReport                  = autoware_auto_vehicle_msgs::msg::GearReport;
+  using TurnIndicatorsCommand       = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+  using PathWithLaneId              = autoware_auto_planning_msgs::msg::PathWithLaneId;
+  using SteeringReport              = autoware_auto_vehicle_msgs::msg::SteeringReport;
+  using VelocityReport              = autoware_auto_vehicle_msgs::msg::VelocityReport;
+  using TurnIndicatorsReport        = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
+  using AckermannControlCommand     = autoware_auto_control_msgs::msg::AckermannControlCommand;
+  using AccelWithCovarianceStamped  = geometry_msgs::msg::AccelWithCovarianceStamped;
+
+  SubscriberWrapper<AckermannControlCommand, ThreadSafety::safe> getAckermannControlCommand;
+  SubscriberWrapper<GearCommand,             ThreadSafety::safe> getGearCommandImpl;
+  SubscriberWrapper<TurnIndicatorsCommand,   ThreadSafety::safe> getTurnIndicatorsCommand;
+  SubscriberWrapper<PathWithLaneId,          ThreadSafety::safe> getPathWithLaneId;
+
+  PublisherWrapper<AccelWithCovarianceStamped>  setAcceleration;
+  PublisherWrapper<nav_msgs::msg::Odometry>     setOdometry;
+  PublisherWrapper<SteeringReport>              setSteeringReport;
+  PublisherWrapper<GearReport>                  setGearReport;
+  PublisherWrapper<ControlModeReport>           setControlModeReport;
+  PublisherWrapper<VelocityReport>              setVelocityReport;
+  PublisherWrapper<TurnIndicatorsReport>        setTurnIndicatorsReport;
   // clang-format on
 
+  rclcpp::Service<ControlModeCommand>::SharedPtr control_mode_request_server;
+
   const rclcpp::TimerBase::SharedPtr localization_update_timer;
 
   const rclcpp::TimerBase::SharedPtr vehicle_state_update_timer;
@@ -59,6 +74,8 @@ class AutowareUniverse : public Autoware
 
   std::atomic<bool> is_stop_requested = false;
 
+  std::atomic<std::uint8_t> current_control_mode = ControlModeReport::AUTONOMOUS;
+
   std::atomic<bool> is_thrown = false;
 
   std::exception_ptr thrown;
@@ -82,15 +99,19 @@ class AutowareUniverse : public Autoware
 
   auto updateVehicleState() -> void;
 
-  auto getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand override;
+  auto getGearCommand() const -> GearCommand override;
 
   auto getGearSign() const -> double override;
 
-  auto getVehicleCommand() const -> std::tuple<
-    autoware_auto_control_msgs::msg::AckermannControlCommand,
-    autoware_auto_vehicle_msgs::msg::GearCommand> override;
+  auto getVehicleCommand() const -> std::tuple<AckermannControlCommand, GearCommand> override;
 
   auto getRouteLanelets() const -> std::vector<std::int64_t>;
+
+  auto getControlModeReport() const -> ControlModeReport override;
+
+  auto setAutonomousMode() -> void override;
+
+  auto setManualMode() -> void override;
 };
 
 }  // namespace concealer

external/concealer/include/concealer/field_operator_application.hpp

@@ -154,6 +154,10 @@ class FieldOperatorApplication : public rclcpp::Node
   virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0;
 
   virtual auto setVelocityLimit(double) -> void = 0;
+
+  virtual auto enableAutowareControl() -> void = 0;
+
+  virtual auto disableAutowareControl() -> void = 0;
 };
 }  // namespace concealer
 

external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp

@@ -20,6 +20,7 @@
 #endif
 
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#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_points.hpp>
@@ -72,6 +73,8 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   ServiceWithValidation<autoware_adapi_v1_msgs::srv::SetRoutePoints>              requestSetRoutePoints;
   ServiceWithValidation<tier4_rtc_msgs::srv::AutoModeWithModule>                  requestSetRtcAutoMode;
   ServiceWithValidation<tier4_external_api_msgs::srv::SetVelocityLimit>           requestSetVelocityLimit;
+  ServiceWithValidation<autoware_adapi_v1_msgs::srv::ChangeOperationMode>         requestEnableAutowareControl;
+  ServiceWithValidation<autoware_adapi_v1_msgs::srv::ChangeOperationMode>         requestDisableAutowareControl;
   // clang-format on
 
   tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array;
@@ -132,7 +135,9 @@ class FieldOperatorApplicationFor<AutowareUniverse>
     // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons.
     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)
+    requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this),
+    requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this),
+    requestDisableAutowareControl("/api/operation_mode/disable_autoware_control", *this)
   // clang-format on
   {
   }
@@ -171,6 +176,10 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   auto sendCooperateCommand(const std::string &, const std::string &) -> void override;
 
   auto setVelocityLimit(double) -> void override;
+
+  auto enableAutowareControl() -> void override;
+
+  auto disableAutowareControl() -> void override;
 };
 }  // namespace concealer
 

external/concealer/include/concealer/service_with_validation.hpp

@@ -116,11 +116,12 @@ class ServiceWithValidation
               return;
             } else {
               RCLCPP_ERROR_STREAM(
-                logger, service_name
-                          << " service request was accepted, but ResponseStatus::success is false "
-                          << (service_call_status.message.empty()
-                                ? ""
-                                : " (" + service_call_status.message + ")"));
+                logger, service_name << " service request was accepted, but "
+                                        "ResponseStatus::success is false with error code: "
+                                     << service_call_status.code << ", and message: "
+                                     << (service_call_status.message.empty()
+                                           ? ""
+                                           : " (" + service_call_status.message + ")"));
             }
           } else {
             RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted.");

external/concealer/include/concealer/task_queue.hpp

@@ -40,6 +40,8 @@ class TaskQueue
 
   std::exception_ptr thrown;
 
+  std::atomic<bool> is_exhausted = true;
+
 public:
   explicit TaskQueue();
 

external/concealer/src/autoware_universe.cpp

@@ -30,6 +30,25 @@ AutowareUniverse::AutowareUniverse()
   setControlModeReport("/vehicle/status/control_mode", *this),
   setVelocityReport("/vehicle/status/velocity_status", *this),
   setTurnIndicatorsReport("/vehicle/status/turn_indicators_status", *this),
+  control_mode_request_server(create_service<ControlModeCommand>(
+    "/control/control_mode_request",
+    [this](
+      const ControlModeCommand::Request::SharedPtr request,
+      ControlModeCommand::Response::SharedPtr response) {
+      if (request->mode == ControlModeCommand::Request::AUTONOMOUS) {
+        current_control_mode.store(ControlModeReport::AUTONOMOUS);
+        response->success = true;
+      } else if (request->mode == ControlModeCommand::Request::MANUAL) {
+        /*
+          NOTE:
+            MANUAL request will come when a remote override is triggered.
+            But scenario_simulator_v2 don't support a remote override for now.
+        */
+        response->success = false;
+      } else {
+        response->success = false;
+      }
+    })),
   // Autoware.Universe requires localization topics to send data at 50Hz
   localization_update_timer(rclcpp::create_timer(
     this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })),
@@ -82,7 +101,7 @@ auto AutowareUniverse::getSteeringAngle() const -> double
 auto AutowareUniverse::updateLocalization() -> void
 {
   setAcceleration([this]() {
-    geometry_msgs::msg::AccelWithCovarianceStamped message;
+    AccelWithCovarianceStamped message;
     message.header.stamp = get_clock()->now();
     message.header.frame_id = "/base_link";
     message.accel.accel = current_acceleration.load();
@@ -110,29 +129,25 @@ auto AutowareUniverse::updateLocalization() -> void
 
 auto AutowareUniverse::updateVehicleState() -> void
 {
-  setControlModeReport([this]() {
-    autoware_auto_vehicle_msgs::msg::ControlModeReport message;
-    message.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS;
-    return message;
-  }());
+  setControlModeReport(getControlModeReport());
 
   setGearReport([this]() {
-    autoware_auto_vehicle_msgs::msg::GearReport message;
+    GearReport message;
     message.stamp = get_clock()->now();
     message.report = getGearCommand().command;
     return message;
   }());
 
   setSteeringReport([this]() {
-    autoware_auto_vehicle_msgs::msg::SteeringReport message;
+    SteeringReport message;
     message.stamp = get_clock()->now();
     message.steering_tire_angle = getSteeringAngle();
     return message;
   }());
 
   setVelocityReport([this]() {
     const auto twist = current_twist.load();
-    autoware_auto_vehicle_msgs::msg::VelocityReport message;
+    VelocityReport message;
     message.header.stamp = get_clock()->now();
     message.header.frame_id = "base_link";
     message.longitudinal_velocity = twist.linear.x;
@@ -142,21 +157,17 @@ auto AutowareUniverse::updateVehicleState() -> void
   }());
 
   setTurnIndicatorsReport([this]() {
-    autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport message;
+    TurnIndicatorsReport message;
     message.stamp = get_clock()->now();
     message.report = getTurnIndicatorsCommand().command;
     return message;
   }());
 }
 
-auto AutowareUniverse::getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand
-{
-  return getGearCommandImpl();
-}
+auto AutowareUniverse::getGearCommand() const -> GearCommand { return getGearCommandImpl(); }
 
 auto AutowareUniverse::getGearSign() const -> double
 {
-  using autoware_auto_vehicle_msgs::msg::GearCommand;
   /// @todo Add support for GearCommand::NONE to return 0.0
   /// @sa https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475
   return getGearCommand().command == GearCommand::REVERSE or
@@ -165,9 +176,7 @@ auto AutowareUniverse::getGearSign() const -> double
            : 1.0;
 }
 
-auto AutowareUniverse::getVehicleCommand() const -> std::tuple<
-  autoware_auto_control_msgs::msg::AckermannControlCommand,
-  autoware_auto_vehicle_msgs::msg::GearCommand>
+auto AutowareUniverse::getVehicleCommand() const -> std::tuple<AckermannControlCommand, GearCommand>
 {
   return std::make_tuple(getAckermannControlCommand(), getGearCommand());
 }
@@ -180,4 +189,21 @@ auto AutowareUniverse::getRouteLanelets() const -> std::vector<std::int64_t>
   }
   return ids;
 }
+
+auto AutowareUniverse::getControlModeReport() const -> ControlModeReport
+{
+  ControlModeReport message;
+  message.mode = current_control_mode.load();
+  return message;
+}
+
+auto AutowareUniverse::setAutonomousMode() -> void
+{
+  current_control_mode.store(ControlModeReport::AUTONOMOUS);
+}
+
+auto AutowareUniverse::setManualMode() -> void
+{
+  current_control_mode.store(ControlModeReport::MANUAL);
+}
 }  // namespace concealer

external/concealer/src/field_operator_application_for_autoware_universe.cpp

@@ -481,6 +481,22 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::requestAutoModeForCooperatio
   }
 }
 
+auto FieldOperatorApplicationFor<AutowareUniverse>::enableAutowareControl() -> void
+{
+  task_queue.delay([this]() {
+    auto request = std::make_shared<autoware_adapi_v1_msgs::srv::ChangeOperationMode::Request>();
+    requestEnableAutowareControl(request);
+  });
+}
+
+auto FieldOperatorApplicationFor<AutowareUniverse>::disableAutowareControl() -> void
+{
+  task_queue.delay([this]() {
+    auto request = std::make_shared<autoware_adapi_v1_msgs::srv::ChangeOperationMode::Request>();
+    requestDisableAutowareControl(request);
+  });
+}
+
 auto FieldOperatorApplicationFor<AutowareUniverse>::receiveEmergencyState(
   const tier4_external_api_msgs::msg::Emergency & message) -> void
 {