add control_mode

Signed-off-by: Yoshihiro Kogure <[email protected]>

control_data_collecting_tool/scripts/data_collecting_base_node.py

@@ -16,6 +16,7 @@
 
 from autoware_adapi_v1_msgs.msg import OperationModeState
 from geometry_msgs.msg import AccelWithCovarianceStamped
+from autoware_vehicle_msgs.msg import ControlModeReport
 from nav_msgs.msg import Odometry
 import numpy as np
 from rcl_interfaces.msg import ParameterDescriptor
@@ -108,6 +109,13 @@ def __init__(self, node_name):
             10,
         )
 
+        self.control_mode_subscription_ = self.create_subscription(
+            ControlModeReport,
+            "/vehicle/status/control_mode",
+            self.subscribe_control_mode,
+            10,
+        )
+
         self._present_kinematic_state = None
         self._present_acceleration = None
         self.present_operation_mode_ = None
@@ -166,3 +174,6 @@ def onAcceleration(self, msg):
 
     def subscribe_operation_mode(self, msg):
         self.present_operation_mode_ = msg.mode
+
+    def subscribe_control_mode(self, msg):
+        self._present_control_mode_ = msg.mode

control_data_collecting_tool/scripts/data_collecting_data_counter.py

@@ -242,6 +242,7 @@ def timer_callback_counter(self):
             self._present_kinematic_state is not None
             and self._present_acceleration is not None
             and self.present_operation_mode_ == 3
+            and self._present_control_mode_ == 1 
         ):
             # calculate steer
             angular_z = self._present_kinematic_state.twist.twist.angular.z

control_data_collecting_tool/scripts/data_collecting_rosbag_record.py

@@ -29,6 +29,7 @@
 from rosbag2_py._storage import TopicMetadata
 from rosidl_runtime_py.utilities import get_message
 import yaml
+from autoware_vehicle_msgs.msg import ControlModeReport
 
 
 class MessageWriter:
@@ -138,6 +139,15 @@ def __init__(self):
             self.subscribe_operation_mode,
             10,
         )
+
+        self._present_control_mode_ = None
+        self.control_mode_subscription_ = self.create_subscription(
+            ControlModeReport,
+            "/vehicle/status/control_mode",
+            self.subscribe_control_mode,
+            10,
+        )
+
         self.operation_mode_subscription_
 
         self.timer_period_callback = 1.0
@@ -149,20 +159,23 @@ def __init__(self):
     def subscribe_operation_mode(self, msg):
         self.present_operation_mode_ = msg.mode
 
+    def subscribe_control_mode(self, msg):
+        self._present_control_mode_ = msg.mode
+
     def record_message(self):
-        # Start subscribing to topics and recording if the operation mode is 3(LOCAL)
-        if self.present_operation_mode_ == 3 and not self.subscribed and not self.recording:
+        # Start subscribing to topics and recording if the operation mode is 3(LOCAL) and control mode is 1(AUTONOMOUS)
+        if self.present_operation_mode_ == 3 and self._present_control_mode_ == 1 and not self.subscribed and not self.recording:
             self.writer.create_writer()
             self.writer.subscribe_topics()
             self.subscribed = True
 
-        # Start recording if topics are subscribed and the operation mode is 3
-        if self.present_operation_mode_ == 3 and self.subscribed and not self.recording:
+        # Start recording if topics are subscribed and the operation mode is 3(LOCAL)
+        if self.present_operation_mode_ == 3 and self._present_control_mode_ == 1 and self.subscribed and not self.recording:
             self.writer.start_record()
             self.recording = True
 
-        # Stop recording if the operation mode changes from 3
-        if self.present_operation_mode_ != 3 and self.subscribed and self.recording:
+        # Stop recording if the operation mode changes from 3(LOCAL) 
+        if (self.present_operation_mode_ != 3 or self._present_control_mode_ != 1) and self.subscribed and self.recording:
             self.writer.stop_record()
             self.subscribed = False
             self.recording = False