From afe1b583f33714ccd67b088a0edd452bbe2996dd Mon Sep 17 00:00:00 2001
From: kosuke55 <kosuke.tnp@gmail.com>
Date: Fri, 6 Sep 2024 19:22:01 +0900
Subject: [PATCH] sub operation status

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../autoware_raw_vehicle_cmd_converter/node.hpp  |  4 ++++
 .../vehicle_adaptor/vehicle_adaptor.hpp          |  5 ++++-
 .../launch/raw_vehicle_converter.launch.xml      |  2 ++
 .../package.xml                                  |  1 +
 .../src/node.cpp                                 | 16 ++++++++++------
 .../src/vehicle_adaptor/vehicle_adaptor.cpp      |  3 ++-
 6 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp
index ced0c2d124c0b..7d3061671352e 100644
--- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp
+++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp
@@ -26,6 +26,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_vehicle_msgs/msg/steering_report.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
@@ -47,6 +48,7 @@ using tier4_vehicle_msgs::msg::ActuationStatusStamped;
 using TwistStamped = geometry_msgs::msg::TwistStamped;
 using Odometry = nav_msgs::msg::Odometry;
 using Steering = autoware_vehicle_msgs::msg::SteeringReport;
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 class DebugValues
 {
@@ -89,6 +91,8 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
     this, "~/input/odometry"};
   autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> sub_accel_{
     this, "~/input/accel"};
+  autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
+    this, "~/input/operation_mode"};
 
   rclcpp::TimerBase::SharedPtr timer_;
 
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp
index 2dbd7af0010f5..d4521c78a9753 100644
--- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp
+++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp
@@ -15,6 +15,7 @@
 #ifndef AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_
 #define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_control_msgs/msg/control.hpp>
 #include <autoware_vehicle_msgs/msg/steering_report.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
@@ -23,6 +24,7 @@
 namespace autoware::raw_vehicle_cmd_converter
 {
 
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_control_msgs::msg::Control;
 using autoware_vehicle_msgs::msg::SteeringReport;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
@@ -35,7 +37,8 @@ class VehicleAdaptor
   Control compensate(
     const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry,
     [[maybe_unused]] const AccelWithCovarianceStamped & accel,
-    [[maybe_unused]] const double steering);
+    [[maybe_unused]] const double steering,
+    [[maybe_unused]] const OperationModeState & operation_mode);
 
 private:
 };
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml
index d563a75debf98..1bb973971ff48 100644
--- a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml
+++ b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml
@@ -5,6 +5,7 @@
   <arg name="input_accel" default="/localization/acceleration"/>
   <arg name="input_steering" default="/vehicle/status/steering_status"/>
   <arg name="input_actuation_status" default="/vehicle/status/actuation_status"/>
+  <arg name="input_operation_mode_state" default="/system/operation_mode/state"/>
   <arg name="output_actuation_cmd" default="/control/command/actuation_cmd"/>
   <arg name="output_steering_status" default="/vehicle/status/steering_status"/>
   <!-- Parameter -->
@@ -17,6 +18,7 @@
     <remap from="~/input/accel" to="$(var input_accel)"/>
     <remap from="~/input/steering" to="$(var input_steering)"/>
     <remap from="~/input/actuation_status" to="$(var input_actuation_status)"/>
+    <remap from="~input/operation_mode_state" to="$(var input_operation_mode_state)"/>
     <remap from="~/output/actuation_cmd" to="$(var output_actuation_cmd)"/>
     <remap from="~/output/steering_status" to="$(var output_steering_status)"/>
   </node>
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml
index 4ed4fbee7ac81..bdde5078cf37d 100644
--- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml
+++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml
@@ -21,6 +21,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_control_msgs</depend>
   <depend>autoware_vehicle_msgs</depend>
   <depend>geometry_msgs</depend>
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp
index 95c37e6be489f..4472a0e398482 100644
--- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp
+++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp
@@ -139,18 +139,22 @@ void RawVehicleCommandConverterNode::publishActuationCmd()
   }
 
   const auto current_accel = sub_accel_.takeData();
+  const auto current_operation_mode = sub_operation_mode_.takeData();
   if (use_vehicle_adaptor_) {
-    if (!current_accel) {
-      RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "current accel is null");
+    if (!current_accel || !current_operation_mode) {
+      RCLCPP_WARN_EXPRESSION(
+        get_logger(), is_debugging_, "some pointers are null: %s, %s",
+        !current_accel ? "accel" : "", !current_operation_mode ? "operation_mode" : "");
       return;
     }
   }
 
   /* compensate control command if vehicle adaptor is enabled */
-  const Control control_cmd = use_vehicle_adaptor_ ? vehicle_adaptor_.compensate(
-                                                       *control_cmd_ptr_, *current_odometry_,
-                                                       *current_accel, *current_steer_ptr_)
-                                                   : *control_cmd_ptr_;
+  const Control control_cmd = use_vehicle_adaptor_
+                                ? vehicle_adaptor_.compensate(
+                                    *control_cmd_ptr_, *current_odometry_, *current_accel,
+                                    *current_steer_ptr_, *current_operation_mode)
+                                : *control_cmd_ptr_;
 
   /* calculate actuation command */
   double desired_accel_cmd = 0.0;
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp
index 736630662acdd..b8a5697a88418 100644
--- a/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp
+++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp
@@ -20,7 +20,8 @@ namespace autoware::raw_vehicle_cmd_converter
 {
 Control VehicleAdaptor::compensate(
   const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry,
-  [[maybe_unused]] const AccelWithCovarianceStamped & accel, [[maybe_unused]] const double steering)
+  [[maybe_unused]] const AccelWithCovarianceStamped & accel, [[maybe_unused]] const double steering,
+  [[maybe_unused]] const OperationModeState & operation_mode)
 {
   // TODO(someone): implement the control command compensation
   Control output_control_cmd = input_control_cmd;