sub operation status

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

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_state"};
 
   rclcpp::TimerBase::SharedPtr timer_;
 

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:
 };

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>

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_interpolation</depend>
   <depend>autoware_vehicle_msgs</depend>

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;

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;