feat(control_data_collecting_tool): add trajectory inside circle and lanelet2 trajectory

control_data_collecting_tool/CMakeLists.txt

@@ -4,39 +4,53 @@ project(control_data_collecting_tool)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+# Configure Qt5
+find_package(Qt5 REQUIRED COMPONENTS Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+
+# Find necessary ROS 2 packages
+find_package(rviz_common REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+
+# Build the library as a plugin
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/data_collecting_area_selection.cpp
+  src/data_collecting_goal_pose.cpp
+)
+
+# Link necessary libraries
+ament_target_dependencies(${PROJECT_NAME}
+  Qt5
+  rviz_common
+  pluginlib
+  rclcpp
+)
+
+# Export the plugin XML file
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+# Install scripts
 install(PROGRAMS
   scripts/data_collecting_pure_pursuit_trajectory_follower.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/rosbag_play.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
+# Install other configurations and files
 install(
   DIRECTORY config/
   DESTINATION share/${PROJECT_NAME}/config
 )
 
-find_package(Qt5 REQUIRED Core Widgets)
-set(QT_LIBRARIES Qt5::Widgets)
-
-set(CMAKE_AUTOMOC ON)
-
-
-ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/data_collecting_area_selection.cpp
-)
-
-target_link_libraries(${PROJECT_NAME}
-  ${QT_LIBRARIES})
-
-pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
-
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
+# Build configuration for the package
 ament_auto_package(
   INSTALL_TO_SHARE
   plugins

control_data_collecting_tool/config/common_param.yaml

@@ -0,0 +1,42 @@
+/**:
+  ros__parameters:
+    LOAD_ROSBAG2_FILES: true
+
+    # COURSE_NAME: eight_course
+    # COURSE_NAME: u_shaped_return
+    # COURSE_NAME: straight_line_positive
+    # COURSE_NAME: straight_line_negative
+    COURSE_NAME: reversal_loop_circle
+    # COURSE_NAME: along_road
+
+    NUM_BINS_V: 10
+    NUM_BINS_STEER: 20
+    NUM_BINS_A: 10
+    V_MIN: 0.0
+    V_MAX: 11.5
+    STEER_MIN: -0.6
+    STEER_MAX: 0.6
+    A_MIN: -1.0
+    A_MAX: 1.0
+
+    max_lateral_accel: 2.0
+    lateral_error_threshold: 2.79
+    yaw_error_threshold: 0.75
+    velocity_limit_by_tracking_error: 1.0
+    mov_ave_window: 50
+    target_longitudinal_velocity: 6.0
+
+    pure_pursuit_type: linearized
+    # pure_pursuit_type: naive
+    wheel_base: 2.79
+    acc_kp: 1.0
+    lookahead_time: 2.0
+    min_lookahead: 2.0
+    linearized_pure_pursuit_steer_kp_param: 2.0
+    linearized_pure_pursuit_steer_kd_param: 2.0
+    stop_acc: -2.0
+    stop_jerk_lim: 5.0
+    lon_acc_lim: 1.5
+    lon_jerk_lim: 0.5
+    steer_lim: 0.6
+    steer_rate_lim: 0.6

control_data_collecting_tool/config/course_param/along_road_param.yaml

@@ -0,0 +1,28 @@
+/**:
+  ros__parameters:
+    # Course Specific Parameters
+    velocity_on_curve: 3.5
+    stopping_distance: 15.0
+    course_width: 1.5
+
+    # Smoothing window for calculating curvature
+    smoothing_window: 100
+
+    # Data Collection Range
+    COLLECTING_DATA_V_MIN: 0.5
+    COLLECTING_DATA_V_MAX: 8.0
+    COLLECTING_DATA_A_MIN: -1.0
+    COLLECTING_DATA_A_MAX: 1.0
+
+    # Noise Parameters
+    longitudinal_velocity_noise_amp: 0.01
+    longitudinal_velocity_noise_min_period: 5.0
+    longitudinal_velocity_noise_max_period: 20.0
+
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 10.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/config/course_param/eight_course_param.yaml

@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    # Course Specific Parameters
+    velocity_on_curve: 4.5
+    # Smoothing window for trajectory
+    smoothing_window: 400
+
+    # Data Collection Range
+    COLLECTING_DATA_V_MIN: 0.5
+    COLLECTING_DATA_V_MAX: 8.0
+    COLLECTING_DATA_A_MIN: -1.0
+    COLLECTING_DATA_A_MAX: 1.0
+
+    # Noise Parameters
+    longitudinal_velocity_noise_amp: 0.01
+    longitudinal_velocity_noise_min_period: 5.0
+    longitudinal_velocity_noise_max_period: 20.0
+
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 5.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml

@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    # Course Specific Parameters
+    trajectory_radius: 35.0
+    enclosing_radius: 40.0
+    look_ahead_distance: 15.0
+
+    # Data Collection Range
+    COLLECTING_DATA_V_MIN: 0.5
+    COLLECTING_DATA_V_MAX: 8.0
+    COLLECTING_DATA_A_MIN: -1.0
+    COLLECTING_DATA_A_MAX: 1.0
+
+    # Noise Parameters
+    longitudinal_velocity_noise_amp: 0.01
+    longitudinal_velocity_noise_min_period: 5.0
+    longitudinal_velocity_noise_max_period: 20.0
+
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 5.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/config/course_param/straight_line_negative_param.yaml

@@ -0,0 +1,23 @@
+/**:
+  ros__parameters:
+    # Course Specific Parameters
+    stopping_buffer_distance: 10.0
+
+    # Data Collection Range
+    COLLECTING_DATA_V_MIN: 0.5
+    COLLECTING_DATA_V_MAX: 8.0
+    COLLECTING_DATA_A_MIN: -1.0
+    COLLECTING_DATA_A_MAX: 1.0
+
+    # Noise Parameters
+    longitudinal_velocity_noise_amp: 0.01
+    longitudinal_velocity_noise_min_period: 5.0
+    longitudinal_velocity_noise_max_period: 20.0
+
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 5.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/config/course_param/straight_line_positive_param.yaml

@@ -0,0 +1,23 @@
+/**:
+  ros__parameters:
+    # Course Specific Parameters
+    stopping_buffer_distance: 10.0
+
+    # Data Collection Range
+    COLLECTING_DATA_V_MIN: 0.5
+    COLLECTING_DATA_V_MAX: 8.0
+    COLLECTING_DATA_A_MIN: -1.0
+    COLLECTING_DATA_A_MAX: 1.0
+
+    # Noise Parameters
+    longitudinal_velocity_noise_amp: 0.01
+    longitudinal_velocity_noise_min_period: 5.0
+    longitudinal_velocity_noise_max_period: 20.0
+
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 5.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/config/course_param/u_shaped_return_param.yaml

@@ -0,0 +1,23 @@
+/**:
+  ros__parameters:
+    # Course Specific Parameters
+    velocity_on_curve: 4.5
+
+    # Data Collection Range
+    COLLECTING_DATA_V_MIN: 0.5
+    COLLECTING_DATA_V_MAX: 8.0
+    COLLECTING_DATA_A_MIN: -1.0
+    COLLECTING_DATA_A_MAX: 1.0
+
+    # Noise Parameters
+    longitudinal_velocity_noise_amp: 0.01
+    longitudinal_velocity_noise_min_period: 5.0
+    longitudinal_velocity_noise_max_period: 20.0
+
+    acc_noise_amp: 0.01
+    acc_noise_min_period: 5.0
+    acc_noise_max_period: 20.0
+
+    steer_noise_amp: 0.01
+    steer_noise_min_period: 5.0
+    steer_noise_max_period: 20.0

control_data_collecting_tool/launch/control_data_collecting_tool.launch.py

@@ -19,31 +19,61 @@
 from ament_index_python.packages import get_package_share_directory
 import launch
 from launch import LaunchService
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+import yaml
+
+
+# Load yaml file
+def get_course_name(yaml_file_path):
+    with open(yaml_file_path, "r") as file:
+        data = yaml.safe_load(file)
+
+    # get 'COURSE_NAME'
+    course_name = data.get("/**", {}).get("ros__parameters", {}).get("COURSE_NAME", None)
+    return course_name
 
 
 def generate_launch_description():
+    # Define the argument for map_path
+    DeclareLaunchArgument("map_path", description="Path to the map directory")
+    # Use the map_path argument in parameters
+    map_path = LaunchConfiguration("map_path")
+
+    # Get the path to the common param file
     package_share_directory = get_package_share_directory("control_data_collecting_tool")
-    param_file_path = os.path.join(package_share_directory, "config", "param.yaml")
+    common_param_file_path = os.path.join(package_share_directory, "config", "common_param.yaml")
+
+    # Get the path to the course-specific param file
+    course_name = get_course_name(common_param_file_path)
+    course_specific_param_file_path = os.path.join(
+        package_share_directory, "config/course_param", course_name + "_param.yaml"
+    )
+
     return launch.LaunchDescription(
         [
             Node(
                 package="control_data_collecting_tool",
                 executable="data_collecting_pure_pursuit_trajectory_follower.py",
                 name="data_collecting_pure_pursuit_trajectory_follower",
-                parameters=[param_file_path],
+                parameters=[common_param_file_path],
             ),
             Node(
                 package="control_data_collecting_tool",
                 executable="data_collecting_trajectory_publisher.py",
                 name="data_collecting_trajectory_publisher",
-                parameters=[param_file_path],
+                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=[param_file_path],
+                parameters=[common_param_file_path],
             ),
             Node(
                 package="control_data_collecting_tool",
@@ -54,7 +84,7 @@ def generate_launch_description():
                 package="control_data_collecting_tool",
                 executable="data_collecting_data_counter.py",
                 name="data_collecting_data_counter",
-                parameters=[param_file_path],
+                parameters=[common_param_file_path],
             ),
         ]
     )