feat(control_data_collecting_tool): add constant acceleration/actuati…

…on mode (#185) * Add actuation mode Signed-off-by: Yoshihiro Kogure <[email protected]> * Temporarily added data_collecting_pure_pursuit_trajectory_follower.py Signed-off-by: Yoshihiro Kogure <[email protected]> * Remove data_collecting_pure_pursuit_trajectory_follower.py Signed-off-by: Yoshihiro Kogure <[email protected]> * Add constant cmd scripts Signed-off-by: Yoshihiro Kogure <[email protected]> * Add pedal threshold for data counting Signed-off-by: Yoshihiro Kogure <[email protected]> * pre-commit run Signed-off-by: Yoshihiro Kogure <[email protected]> * Add some comments Signed-off-by: Yoshihiro Kogure <[email protected]> * Update install programs Signed-off-by: Yoshihiro Kogure <[email protected]> * Update params Signed-off-by: Yoshihiro Kogure <[email protected]> * pre-commit run Signed-off-by: Yoshihiro Kogure <[email protected]> * Update README Signed-off-by: Yoshihiro Kogure <[email protected]> * The bullet points were changed from numbers to letters. Signed-off-by: Yoshihiro Kogure <[email protected]> * Update print message Signed-off-by: Yoshihiro Kogure <[email protected]> * Fix bug and typo Signed-off-by: Yoshihiro Kogure <[email protected]> * Update README Signed-off-by: Yoshihiro Kogure <[email protected]> * pre-commit run Signed-off-by: Yoshihiro Kogure <[email protected]> * Update control_data_collecting_tool/README.md --------- Signed-off-by: Yoshihiro Kogure <[email protected]> Co-authored-by: Kosuke Takeuchi <[email protected]>
autowarefoundation · Jan 9, 2025 · e4d32ad · e4d32ad
1 parent 6492e0f
commit e4d32ad
Show file tree

Hide file tree

Showing 27 changed files with 2,645 additions and 174 deletions.
diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt
@@ -33,12 +33,15 @@ pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.
 
 # Install scripts
 install(PROGRAMS
-  scripts/data_collecting_pure_pursuit_trajectory_follower.py
+  scripts/data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py
+  scripts/data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py
   scripts/data_collecting_trajectory_publisher.py
   scripts/data_collecting_plotter.py
   scripts/data_collecting_rosbag_record.py
   scripts/data_collecting_data_counter.py
   scripts/data_collecting_base_node.py
+  scripts/calibration/data_collecting_acceleration_cmd.py
+  scripts/calibration/data_collecting_actuation_cmd.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md
diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml
@@ -1,6 +1,11 @@
 /**:
   ros__parameters:
-    LOAD_ROSBAG2_FILES: true
+    CONTROL_MODE: acceleration_cmd
+    # CONTROL_MODE: actuation_cmd
+    # CONTROL_MODE: external_acceleration_cmd
+    # CONTROL_MODE: external_actuation_cmd
+
+    LOAD_ROSBAG2_FILES: false
 
     # COURSE_NAME: eight_course
     # COURSE_NAME: u_shaped_return
@@ -14,6 +19,8 @@
     NUM_BINS_A: 10
     NUM_BINS_ABS_STEER_RATE: 5
     NUM_BINS_JERK: 10
+    NUM_BINS_ACCEL_PEDAL_INPUT: 8
+    NUM_BINS_BRAKE_PEDAL_INPUT: 16
     V_MIN: 0.0
     V_MAX: 11.5
     STEER_MIN: -0.6
@@ -24,6 +31,12 @@
     ABS_STEER_RATE_MAX: 0.3
     JERK_MIN: -0.5
     JERK_MAX: 0.5
+    ACCEL_PEDAL_INPUT_MAX: 0.4
+    ACCEL_PEDAL_INPUT_MIN: 0.0
+    BRAKE_PEDAL_INPUT_MAX: 0.8
+    BRAKE_PEDAL_INPUT_MIN: 0.0
+
+    STEER_THRESHOLD_FOR_PEDAL_INPUT_COUNT: 0.2
 
     MASK_NAME: default
     VEL_ACC_THRESHOLD: 40

diff --git a/control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml b/control_data_collecting_tool/config/constant_cmd_param/acceleration_cmd_param.yaml
@@ -0,0 +1,17 @@
+/data_collecting_acceleration_cmd:
+  ros__parameters:
+    # params for data_collecting_acceleration_cmd
+    TARGET_VELOCITY: 11.80
+    TARGET_ACCELERATION_FOR_DRIVE: 1.5
+    TARGET_ACCELERATION_FOR_BRAKE: -1.5
+    TARGET_JERK_FOR_DRIVE: 1.5
+    TARGET_JERK_FOR_BRAKE: -1.5
+    MIN_ACCEL: -5.0
+    MAX_ACCEL: 2.0
+    topics:
+      - /vehicle/status/velocity_status
+      - /control/command/actuation_cmd
+      - /sensing/imu/imu_data
+      - /vehicle/status/control_mode
+    validation_nodes:
+      - /data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd
diff --git a/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml b/control_data_collecting_tool/config/constant_cmd_param/actuation_cmd_param.yaml
@@ -0,0 +1,16 @@
+/data_collecting_actuation_cmd:
+  ros__parameters:
+    # params for data_collecting_actuation_cmd
+    TARGET_VELOCITY: 11.80
+    TARGET_ACTUATION_FOR_ACCEL: 0.3
+    TARGET_ACTUATION_FOR_BRAKE: 0.5
+    MAX_ACCEL_PEDAL: 0.5
+    MIN_BRAKE_PEDAL: 0.8
+    topics:
+      - /vehicle/status/velocity_status
+      - /control/command/control_cmd
+      - /sensing/imu/imu_data
+      - /vehicle/status/control_mode
+    validation_nodes:
+      - /raw_vehicle_cmd_converter
+      - /data_collecting_pure_pursuit_trajectory_follower_actuation_cmd
diff --git a/control_data_collecting_tool/config/topics.yaml b/control_data_collecting_tool/config/topics.yaml
@@ -2,9 +2,7 @@ topics:
   - /localization/kinematic_state
   - /localization/acceleration
   - /vehicle/status/steering_status
-  - /sensing/imu/imu_data
   - /system/operation_mode/state
-  - /vehicle/status/control_mode
+  - /control/command/control_cmd
   - /external/selected/control_cmd
-  - /external/selected/gear_cmd
-  - /data_collecting_trajectory
+  - /control/command/actuation_cmd
diff --git a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py
@@ -35,6 +35,15 @@ def get_course_name(yaml_file_path):
     return course_name
 
 
+def get_control_mode(yaml_file_path):
+    with open(yaml_file_path, "r") as file:
+        data = yaml.safe_load(file)
+
+    # get 'COURSE_NAME'
+    control_mode = data.get("/**", {}).get("ros__parameters", {}).get("CONTROL_MODE", None)
+    return control_mode
+
+
 def generate_launch_description():
     # Define the argument for map_path
     map_path_arg = DeclareLaunchArgument(
@@ -45,6 +54,14 @@ def generate_launch_description():
     # Use the map_path argument in parameters
     map_path = LaunchConfiguration("map_path")
 
+    # Define the path to the accel/brake map file
+    map_path_arg = DeclareLaunchArgument(
+        "accel_brake_map_path",
+        description="Path to the accel/brake map directory (optional; defaults to None if not provided).",
+        default_value="",  # Default to None
+    )
+    accel_brake_map_path = LaunchConfiguration("accel_brake_map_path")
+
     # Get the path to the common param file
     package_share_directory = get_package_share_directory("control_data_collecting_tool")
     common_param_file_path = os.path.join(package_share_directory, "config", "common_param.yaml")
@@ -55,44 +72,91 @@ def generate_launch_description():
         package_share_directory, "config/course_param", course_name + "_param.yaml"
     )
 
-    return launch.LaunchDescription(
-        [
-            map_path_arg,
-            Node(
-                package="control_data_collecting_tool",
-                executable="data_collecting_pure_pursuit_trajectory_follower.py",
-                name="data_collecting_pure_pursuit_trajectory_follower",
-                parameters=[common_param_file_path],
-            ),
-            Node(
-                package="control_data_collecting_tool",
-                executable="data_collecting_trajectory_publisher.py",
-                name="data_collecting_trajectory_publisher",
-                parameters=[
-                    common_param_file_path,
-                    course_specific_param_file_path,
-                    {"map_path": map_path},
-                ],
-            ),
-            Node(
-                package="control_data_collecting_tool",
-                executable="data_collecting_plotter.py",
-                name="data_collecting_plotter",
-                parameters=[common_param_file_path],
-            ),
-            Node(
-                package="control_data_collecting_tool",
-                executable="data_collecting_rosbag_record.py",
-                name="data_collecting_rosbag_record",
-            ),
-            Node(
-                package="control_data_collecting_tool",
-                executable="data_collecting_data_counter.py",
-                name="data_collecting_data_counter",
-                parameters=[common_param_file_path],
-            ),
-        ]
-    )
+    # Get the control mode
+    control_mode = get_control_mode(common_param_file_path)
+
+    if control_mode == "acceleration_cmd" or control_mode == "external_acceleration_cmd":
+        return launch.LaunchDescription(
+            [
+                map_path_arg,
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd.py",
+                    name="data_collecting_pure_pursuit_trajectory_follower_acceleration_cmd",
+                    parameters=[common_param_file_path],
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_trajectory_publisher.py",
+                    name="data_collecting_trajectory_publisher",
+                    parameters=[
+                        common_param_file_path,
+                        course_specific_param_file_path,
+                        {"map_path": map_path},
+                    ],
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_plotter.py",
+                    name="data_collecting_plotter",
+                    parameters=[common_param_file_path],
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_rosbag_record.py",
+                    name="data_collecting_rosbag_record",
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_data_counter.py",
+                    name="data_collecting_data_counter",
+                    parameters=[common_param_file_path],
+                ),
+            ]
+        )
+
+    elif control_mode == "actuation_cmd" or control_mode == "external_actuation_cmd":
+        return launch.LaunchDescription(
+            [
+                map_path_arg,
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_pure_pursuit_trajectory_follower_actuation_cmd.py",
+                    name="data_collecting_pure_pursuit_trajectory_follower_actuation_cmd",
+                    parameters=[
+                        common_param_file_path,
+                        {"accel_brake_map_path": accel_brake_map_path},
+                    ],
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_trajectory_publisher.py",
+                    name="data_collecting_trajectory_publisher",
+                    parameters=[
+                        common_param_file_path,
+                        course_specific_param_file_path,
+                        {"map_path": map_path},
+                    ],
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_plotter.py",
+                    name="data_collecting_plotter",
+                    parameters=[common_param_file_path],
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_rosbag_record.py",
+                    name="data_collecting_rosbag_record",
+                ),
+                Node(
+                    package="control_data_collecting_tool",
+                    executable="data_collecting_data_counter.py",
+                    name="data_collecting_data_counter",
+                    parameters=[common_param_file_path],
+                ),
+            ]
+        )
 
 
 if __name__ == "__main__":

diff --git a/control_data_collecting_tool/resource/looking_ahead.png b/control_data_collecting_tool/resource/looking_ahead.png
diff --git a/control_data_collecting_tool/scripts/accel_brake_map.py b/control_data_collecting_tool/scripts/accel_brake_map.py
@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# 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.
+
+import numpy as np
+import pandas as pd
+from scipy.interpolate import RegularGridInterpolator
+
+
+class MapConverter:
+    def __init__(self, path_to_map):
+        # Load map data from CSV and initialize grid values
+        map_df = pd.read_csv(path_to_map, delimiter=",", header=0)
+        self.map_grid = map_df.to_numpy()[:, 1:]
+
+        # Extract velocity grid from column headers
+        self.velocity_grid_map = np.array(map_df.columns[1:], dtype=float)
+        self.max_vel_of_map = self.velocity_grid_map[-1]
+        self.min_vel_of_map = self.velocity_grid_map[0]
+
+        # Extract pedal grid from the first column
+        self.pedal_grid_map = map_df.to_numpy()[:, 0]
+        self.max_pedal_of_map = self.pedal_grid_map[-1]
+        self.min_pedal_of_map = self.pedal_grid_map[0]
+
+        # Create an interpolator for the map grid
+        self.map_grid_interpolator = RegularGridInterpolator(
+            (self.pedal_grid_map, self.velocity_grid_map), self.map_grid
+        )
+
+    def pedal_to_accel_input(self, pedal, velocity):
+        # Convert pedal input and velocity to acceleration input using interpolation
+        pedal = np.clip(pedal, self.min_pedal_of_map, self.max_pedal_of_map)
+        velocity = np.clip(velocity, self.min_vel_of_map, self.max_vel_of_map)
+        accel_input = self.map_grid_interpolator([pedal, velocity])[0]
+
+        return accel_input
+
+    def accel_input_to_pedal(self, accel_input, velocity):
+        # Convert acceleration input and velocity to pedal input
+        velocity = np.clip(velocity, self.min_vel_of_map, self.max_vel_of_map)
+
+        # Find velocity interval for interpolation
+        vel_idx = 0
+        alpha = 0.0
+        for i in range(len(self.velocity_grid_map) - 1):
+            vel_left_grid = self.velocity_grid_map[i]
+            vel_right_grid = self.velocity_grid_map[i + 1]
+
+            if vel_left_grid <= velocity <= vel_right_grid:
+                vel_idx = i
+                alpha = (velocity - vel_left_grid) / (vel_right_grid - vel_left_grid)
+
+        # Interpolate acceleration input map for given velocity
+        accel_input_map = (
+            alpha * self.map_grid[:, vel_idx] + (1 - alpha) * self.map_grid[:, vel_idx + 1]
+        )
+
+        # Clip acceleration input within the bounds of the map
+        min_accel_map = np.min([accel_input_map[0], accel_input_map[-1]])
+        max_accel_map = np.max([accel_input_map[0], accel_input_map[-1]])
+        accel_input = np.clip(accel_input, min_accel_map, max_accel_map)
+
+        # Find corresponding pedal input using interpolation
+        actuation_input = 0.0
+        for i in range(len(accel_input_map) - 1):
+            if accel_input_map[i] <= accel_input_map[i + 1]:
+                # Increasing interval
+                if accel_input_map[i] <= accel_input <= accel_input_map[i + 1]:
+                    delta_accel_input = accel_input_map[i + 1] - accel_input_map[i]
+                    if abs(delta_accel_input) < 1e-3:
+                        actuation_input = self.pedal_grid_map[i]
+                    else:
+                        beta = (accel_input - accel_input_map[i]) / delta_accel_input
+                        actuation_input = (
+                            beta * self.pedal_grid_map[i + 1] + (1 - beta) * self.pedal_grid_map[i]
+                        )
+            elif accel_input_map[i + 1] < accel_input_map[i]:
+                # Decreasing interval
+                sign = -1
+                if accel_input_map[i + 1] <= accel_input <= accel_input_map[i]:
+                    delta_accel_input = accel_input_map[i] - accel_input_map[i + 1]
+                    if abs(delta_accel_input) < 1e-3:
+                        actuation_input = sign * self.pedal_grid_map[i]
+                    else:
+                        beta = (accel_input - accel_input_map[i + 1]) / delta_accel_input
+                        actuation_input = sign * (
+                            beta * self.pedal_grid_map[i] + (1 - beta) * self.pedal_grid_map[i + 1]
+                        )
+
+        return actuation_input
+
+
+class AccelBrakeMapConverter:
+    def __init__(self, path_to_accel_map, path_to_brake_map):
+        # Initialize converters for acceleration and brake maps
+        self.accel_map_converter = MapConverter(path_to_accel_map)
+        self.brake_map_converter = MapConverter(path_to_brake_map)
+
+    def convert_accel_input_to_actuation_cmd(self, accel_input, velocity):
+        # Convert acceleration input to actuation command
+        accel_actuation_cmd = self.accel_map_converter.accel_input_to_pedal(accel_input, velocity)
+        brake_actuation_cmd = self.brake_map_converter.accel_input_to_pedal(accel_input, velocity)
+
+        return accel_actuation_cmd - brake_actuation_cmd
+
+    def convert_actuation_cmd_to_accel_input(self, actuation_cmd, velocity):
+        # Convert actuation command to acceleration input
+        accel_input = 0.0
+        if actuation_cmd > 0.0:
+            # Positive command corresponds to acceleration
+            accel_input = self.accel_map_converter.pedal_to_accel_input(
+                abs(actuation_cmd), velocity
+            )
+        if actuation_cmd <= 0.0:
+            # Negative command corresponds to braking
+            accel_input = self.brake_map_converter.pedal_to_accel_input(
+                abs(actuation_cmd), velocity
+            )
+
+        return accel_input