use the mode to avoid waiting manually. But the 10 seconds delay failed...

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

Author: HansOersted

Diff for: experiment/unit_step_steer/CMakeLists.txt

@@ -4,10 +4,13 @@ project(unit_step_steer)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+find_package(autoware_adapi_v1_msgs REQUIRED)
+
 add_executable(unit_step_steer src/step_unit_pub.cpp)
 ament_target_dependencies(unit_step_steer
   rclcpp
   autoware_auto_control_msgs
+  autoware_adapi_v1_msgs
 )
 
 install(TARGETS

Diff for: experiment/unit_step_steer/src/step_unit_pub.cpp

@@ -1,5 +1,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
+#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" // src/autoware/autoware_adapi_msgs/autoware_adapi_v1_msgs/operation_mode/msg/OperationModeState.msg
 #include <limits>
 #include <chrono>
 #include "rclcpp/qos.hpp"
@@ -18,42 +19,62 @@ class ControlCommandPublisher : public rclcpp::Node
         timer_ = this->create_wall_timer(
             std::chrono::milliseconds(100),
             std::bind(&ControlCommandPublisher::publish_control_command, this));
+        // Set up the subscriber for the operation mode state
+        operation_mode_subscriber_ = this->create_subscription<autoware_adapi_v1_msgs::msg::OperationModeState>(
+            "/api/operation_mode/state",
+            10,
+            std::bind(&ControlCommandPublisher::operation_mode_callback, this, std::placeholders::_1));
     }
 
 private:
     void publish_control_command()
     {
         auto control_command = autoware_auto_control_msgs::msg::AckermannControlCommand();
-        rclcpp::Time now = this->get_clock()->now();
 
-        if ((now - start_time_).seconds() < 10.0) {
-            // For the first 10 seconds, publish zeros
+        if (operation_mode_ != 2) {
             control_command.lateral.steering_tire_angle = 0.0;
             control_command.lateral.steering_tire_rotation_rate = 0.0;
             control_command.longitudinal.acceleration = 0.0;
             control_command.longitudinal.jerk = 0.0;
             control_command.longitudinal.speed = 0.001;
+            start_time_ = this->get_clock()->now();
         } else {
-            // After 10 seconds, publish the actual values
-            control_command.lateral.steering_tire_angle = 0.5;
-            control_command.lateral.steering_tire_rotation_rate = std::numeric_limits<double>::infinity();
-            control_command.longitudinal.acceleration = 0.0; // Set to your desired value
-            control_command.longitudinal.jerk = 0.0; // Set to your desired value
-            control_command.longitudinal.speed = 0.001; // Set to your desired value
+            if ((this->get_clock()->now() - start_time_).seconds() < 10.0) {
+                // For the first 10 seconds, publish zeros
+                control_command.lateral.steering_tire_angle = 0.0;
+                control_command.lateral.steering_tire_rotation_rate = 0.0;
+                control_command.longitudinal.acceleration = 0.0;
+                control_command.longitudinal.jerk = 0.0;
+                control_command.longitudinal.speed = 0.001;
+            } else {
+                // After 10 seconds, publish the actual values
+                control_command.lateral.steering_tire_angle = 0.5;
+                control_command.lateral.steering_tire_rotation_rate = std::numeric_limits<double>::infinity();
+                control_command.longitudinal.acceleration = 0.0; // Set to your desired value
+                control_command.longitudinal.jerk = 0.0; // Set to your desired value
+                control_command.longitudinal.speed = 0.001; // Set to your desired value
+            }
         }
 
         // Set the time stamp
-        control_command.stamp = now;
-        control_command.lateral.stamp = now;
-        control_command.longitudinal.stamp = now;
+        control_command.stamp = this->get_clock()->now();
+        control_command.lateral.stamp = this->get_clock()->now();
+        control_command.longitudinal.stamp = this->get_clock()->now();
 
-        RCLCPP_INFO(this->get_logger(), "Publishing control command at time '%u.%09u'", control_command.stamp.sec, control_command.stamp.nanosec);
+        // RCLCPP_INFO(this->get_logger(), "Publishing control command at time '%u.%09u'", control_command.stamp.sec, control_command.stamp.nanosec);
         publisher_->publish(control_command);
     }
+    
+    void operation_mode_callback(const autoware_adapi_v1_msgs::msg::OperationModeState::SharedPtr msg)
+    {
+        operation_mode_ = msg->mode;
+    }
 
     rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr publisher_;
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::Time start_time_;
+    rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr operation_mode_subscriber_;
+    int operation_mode_{0};
 };
 
 int main(int argc, char* argv[])