Improve simulated manual override with FollowTrajectoryAction

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/CMakeLists.txt

@@ -19,8 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/execute.cpp
   src/field_operator_application.cpp
   src/field_operator_application_for_autoware_universe.cpp
-  src/is_package_exists.cpp
-  src/task_queue.cpp)
+  src/is_package_exists.cpp)
 
 target_link_libraries(${PROJECT_NAME}
   atomic)

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_state_dispatcher.hpp

@@ -0,0 +1,130 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONCEALER__AUTOWARE_STATE_DISPATCHER_HPP_
+#define CONCEALER__AUTOWARE_STATE_DISPATCHER_HPP_
+
+#include <functional>
+#include <future>
+#include <iostream>
+#include <rclcpp/rclcpp.hpp>
+#include <unordered_map>
+#include <utility>
+
+template <typename TAutowareState>
+class AutowareStateDispatcher
+{
+public:
+  AutowareStateDispatcher() = default;
+  virtual ~AutowareStateDispatcher() = default;
+
+  void onAutowareState(const TAutowareState & msg, const rclcpp::Time & now)
+  {
+    if (auto pre_message = std::exchange(last_message, msg);
+        getState(pre_message) != getState(msg)) {
+      onStateChanged(getState(pre_message), getState(msg));
+    }
+    executeTask(getState(msg), now);
+  }
+
+  std::string getState(const TAutowareState & message) const { return message.state; }
+
+  void registerTask(
+    const std::string state, std::function<void()> task,
+    rclcpp::Duration interval =
+      rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::max()))
+  {
+    tasks[state].push_back(Task(std::move(task), interval));
+  }
+
+  void registerTaskForCurrentState(
+    std::function<void()> task, rclcpp::Duration interval = rclcpp::Duration::from_nanoseconds(
+                                  std::numeric_limits<rcl_duration_value_t>::max()))
+  {
+    registerTask(getState(last_message), std::move(task), interval);
+  }
+
+private:
+  void onStateChanged(const std::string & pre_state, const std::string & state)
+  {
+    std::cout << "AutowareState changed from " << pre_state << " to " << state << std::endl;
+    tasks.erase(pre_state);  // unregister old tasks when state changes
+  }
+
+  void executeTask(std::string state, const rclcpp::Time & now)
+  {
+    if (auto current_task_list = tasks.find(state); current_task_list != tasks.end()) {
+      for (auto task = current_task_list->second.begin();
+           task != current_task_list->second.end();) {
+        if (task->available) {
+          (*task)(now);
+          ++task;
+        } else {
+          task = current_task_list->second.erase(task);
+        }
+      }
+    }
+  }
+
+  TAutowareState last_message;
+
+  struct Task
+  {
+    std::function<void()> task_function;
+
+    rclcpp::Duration interval;
+
+    rclcpp::Time next_execution;
+
+    std::future<void> future;
+
+    bool available = true;
+
+    Task(std::function<void()> t, rclcpp::Duration i)
+    : task_function(std::move(t)), interval(i), next_execution(rclcpp::Time(0, 0, RCL_ROS_TIME))
+    {
+    }
+
+    void operator()(const rclcpp::Time & now)
+    {
+      if (now >= next_execution) {
+        if (
+          not future.valid() or
+          future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
+          future = std::async(std::launch::async, task_function);
+          int64_t duration_nanoseconds = now.nanoseconds() - next_execution.nanoseconds();
+          int64_t interval_nanoseconds = interval.nanoseconds();
+          if (interval_nanoseconds != 0) {
+            next_execution =
+              now + rclcpp::Duration::from_nanoseconds(
+                      interval_nanoseconds - (duration_nanoseconds % interval_nanoseconds));
+          } else {
+            next_execution = now;
+          }
+
+          // If the interval is set to the maximum value, the task will not be executed again.
+          if (
+            interval ==
+            rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::max())) {
+            available = false;
+          }
+        }
+      }
+    }
+  };
+
+  std::unordered_map<std::string, std::vector<Task>> tasks;
+};
+
+#endif  // CONCEALER__AUTOWARE_STATE_DISPATCHER_HPP_

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

@@ -27,8 +27,6 @@
 #include <chrono>
 #include <concealer/autoware_stream.hpp>
 #include <concealer/launch.hpp>
-#include <concealer/task_queue.hpp>
-#include <concealer/transition_assertion.hpp>
 #include <concealer/visibility.hpp>
 #include <exception>
 #include <geometry_msgs/msg/accel.hpp>
@@ -67,8 +65,6 @@ class FieldOperatorApplication : public rclcpp::Node
 protected:
   const pid_t process_id = 0;
 
-  TaskQueue task_queue;
-
   bool initialize_was_called = false;
 
   auto stopRequest() noexcept -> void;
@@ -149,11 +145,13 @@ class FieldOperatorApplication : public rclcpp::Node
   virtual auto getTurnIndicatorsCommand() const
     -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 
-  virtual auto rethrow() const noexcept(false) -> void;
-
   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