diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index e224634c..8039f457 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -4,6 +4,34 @@ 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 @@ -11,32 +39,18 @@ install(PROGRAMS 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 diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index db8ca8d3..0a3c1427 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -2,146 +2,238 @@ This package provides tools for automatically collecting data using pure pursuit control within a specified rectangular area. - + ## Overview - This package aims to collect a dataset consisting of control inputs (i.e. `control_cmd`) and observation variables (i.e. `kinematic_state`, `steering_status`, etc). - The collected dataset can be used as training dataset for learning-based controllers, including [smart_mpc](https://github.com/autowarefoundation/autoware.universe/tree/f30c0350861d020ad26a45806ab1334895122fab/control/smart_mpc_trajectory_follower). -- The data collecting approach is as follows: - - Setting a figure-eight target trajectory within the specified rectangular area. +- Data collecting approach is as follows: + - Following the trajectory using a pure pursuit control law. - Adding noises to the trajectory and the control command for data diversity, improving the prediction accuracy of learning model. + - Setting the trajectory from the following types of trajectories ( [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`, `along_road`] ). + + - `COURSE_NAME: eight_course` + + + + - `COURSE_NAME: u_shaped_return` + + + + - `COURSE_NAME: straight_line_positive` or `COURSE_NAME: straight_line_negative` + + ( Both "straight_line_positive" and "straight_line_negative" represent straight line courses, but the direction of travel of the course is reversed.) + + + + - `COURSE_NAME: reversal_loop_circle` -## How to use + Drive within a circle while adding trajectories and collect data. + + + + - `COURSE_NAME: along_road` + + Generate trajectories along the road. This is particularly useful when drawing long straight paths along the road. + + In this course, data collection is conducted only on long straight trajectories, while constant velocity, `velocity_on_curve`, is maintained when driving on sections that include curves. + + The minimum length of these long straight trajectories can be specified using the parameter `minimum_length_of_straight_line` (These two parameters `velocity_on_curve` and `minimum_length_of_straight_line` can be configured in `./config/course_param/along_road_param.yaml`). + + + +## How to Use + + + 1. Launch Autoware. - ```bash - ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit - ``` + ```bash + ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit + ``` 2. Set an initial pose, see [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#2-set-an-initial-pose-for-the-ego-vehicle). -3. Add `DataCollectingAreaSelectionTool` rviz plugin. - - +3. Add the DataCollectingAreaSelectionTool and DataCollectingGoalPlugin RViz plugins by clicking the "+" icon at the top of the RViz window.
4. Launch control_data_collecting_tool. - ```bash - ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py - ``` + ```bash + ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py map_path:=$HOME/autoware_map/sample-map-planning + ``` - - Control data collecting tool automatically records topics included in `config/topics.yaml` when the above command is executed. - The topics will be saved in rosbag2 format in the directory where the above command is executed. + - If you use the `along_road` course, please specify the same map for `map_path` as the one used when launching Autoware. `map_path` is not necessary when using courses other than `along_road`. - - The data from `/localization/kinematic_state` and `/localization/acceleration` located in the directory (rosbag2 format) where the command is executed will be automatically loaded and reflected in the data count for these topics. - (If `LOAD_ROSBAG2_FILES` in `config/param.yaml` is set to `false`, the data is noe loaded.) + - Control data collecting tool automatically records topics included in `config/topics.yaml` when the above command is executed. + Topics will be saved in rosbag2 format in the current directory. + + - The data from `/localization/kinematic_state` and `/localization/acceleration` located in the directory (rosbag2 format) where the command is executed will be automatically loaded and reflected in the data count for these topics. + (If `LOAD_ROSBAG2_FILES` in `config/param.yaml` is set to `false`, the data is not loaded.) 5. Add visualization in rviz: - - `/data_collecting_area` - - Type: Polygon - - `/data_collecting_trajectory_marker_array` - - Type: MarkerArray - - `/data_collecting_lookahead_marker_array` - - Type: MarkerArray + - `/data_collecting_area` + - Type: Polygon + - `/data_collecting_trajectory_marker_array` + - Type: MarkerArray + - `/data_collecting_lookahead_marker_array` + - Type: MarkerArray + +6. The following actions differ depending on the selected course. If you select the trajectory from [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`], proceed to 6.1. If you select the trajectory from [`along_road`], please proceed to 6.2. + + - 6.1 If you choose the trajectory from [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`], select `DataCollectingAreaSelectionTool` plugin. + + + + Highlight the data collecting area by dragging the mouse over it. + + + + > [!NOTE] + > You cannot change the data collecting area while driving. + + - 6.2 If you choose the trajectory from [`along_road`], select `DataCollectingGoalPose` plugin. + + -6. Select `DataCollectingAreaSelectionTool` plugin. + By setting the pose of the goal point, a trajectory is generated on the map. - + - Highlight the data collecting area by dragging the mouse over it. + As soon as the trajectory is generated, the plot with the map and trajectory drawn on it will be created (please see the following picture). + In the sections labeled `velocity = const (velocity_on_curve)` in the legend, the vehicle travels at a constant velocity of `velocity_on_curve`. In the sections labeled `Data collection is conducted`, data collection is performed. - + - > [!NOTE] - > You cannot change the data collecting area while driving. + > [!NOTE] + > You cannot change the goal pose while driving. + > In cases where course generation fails, which can happen under certain conditions, please reposition the vehicle or redraw the goal pose. 7. Click the `LOCAL` button on `OperationMode` in `AutowareStatePanel`. - + - Then, data collecting starts. + Then, data collecting starts. - + 8. If you want to stop data collecting automatic driving, run the following command - ```bash - ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: true" --once - ``` + ```bash + ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: true" --once + ``` - > [!NOTE] - > When the car crosses the green boundary line, a similar stopping procedure will be automatically triggered. + > [!NOTE] + > When the car crosses the green boundary line, a similar stopping procedure will be automatically triggered. 9. If you want to restart data collecting automatic driving, run the following command - ```bash - ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once - ``` + ```bash + ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once + ``` -## Change Courses + -You can change the course by selecting `COURSE_NAME` in `config/param.yaml` from [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`]. +## Parameter + +There are parameters that are common to all trajectories and parameters that are specific to each trajectory. + +### Common Parameters + +ROS 2 parameters which are common in all trajectories (`/config/common_param.yaml`): + +| Name | Type | Description | Default value | +| :--------------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------- | :--------------------- | +| `LOAD_ROSBAG2_FILES` | `bool` | Flag that determines whether to load rosbag2 data or not | true | +| `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`, `along_road`] | `reversal_loop_circle` | +| `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 | +| `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 20 | +| `NUM_BINS_A` | `int` | Number of bins of acceleration in heatmap | 10 | +| `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 | +| `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 | +| `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -0.6 | +| `STEER_MAX` | `double` | Maximum steer in heatmap [rad] | 0.6 | +| `A_MIN` | `double` | Minimum acceleration in heatmap [m/ss] | -1.0 | +| `A_MAX` | `double` | Maximum acceleration in heatmap [m/ss] | 1.0 | +| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 2.00 | +| `lateral_error_threshold` | `double` | Lateral error threshold where applying velocity limit [m/s] | 1.50 | +| `yaw_error_threshold` | `double` | Yaw error threshold where applying velocity limit [rad] | 0.75 | +| `velocity_limit_by_tracking_error` | `double` | Velocity limit applied when tracking error exceeds threshold [m/s] | 1.0 | +| `mov_ave_window` | `int` | Moving average smoothing window size | 50 | +| `target_longitudinal_velocity` | `double` | Target longitudinal velocity [m/s] | 6.0 | +| `pure_pursuit_type` | `string` | Pure pursuit type (`naive` or `linearized` steer control law ) | `linearized` | +| `wheel_base` | `double` | Wheel base [m] | 2.79 | +| `acc_kp` | `double` | Accel command proportional gain | 1.0 | +| `lookahead_time` | `double` | Pure pursuit lookahead time [s] | 2.0 | +| `min_lookahead` | `double` | Pure pursuit minimum lookahead length [m] | 2.0 | +| `linearized_pure_pursuit_steer_kp_param` | `double` | Linearized pure pursuit steering P gain parameter | 2.0 | +| `linearized_pure_pursuit_steer_kd_param` | `double` | Linearized pure pursuit steering D gain parameter | 2.0 | +| `stop_acc` | `double` | Accel command for stopping data collecting driving [m/ss] | -2.0 | +| `stop_jerk_lim` | `double` | Jerk limit for stopping data collecting driving [m/sss] | 5.0 | +| `lon_acc_lim` | `double` | Longitudinal acceleration limit [m/ss] | 1.5 | +| `lon_jerk_lim` | `double` | Longitudinal jerk limit [m/sss] | 0.5 | +| `steer_lim` | `double` | Steering angle limit [rad] | 0.6 | +| `steer_rate_lim` | `double` | Steering angle rate limit [rad/s] | 0.6 | + +The following parameters are common to all trajectories but can be defined individually for each trajectory. (`/config/course_param/COURSE_NAME_param.yaml`): +| Name | Type | Description | Default value | +| :--------------------------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- | +| `COLLECTING_DATA_V_MIN` | `double` | Minimum velocity for data collection [m/s] | 0.5 | +| `COLLECTING_DATA_V_MAX` | `double` | Maximum velocity for data collection [m/s] | 8.0 | +| `COLLECTING_DATA_A_MIN` | `double` | Minimum velocity for data collection [m/ss] | 1.0 | +| `COLLECTING_DATA_A_MAX` | `double` | Maximum velocity for data collection [m/ss] | -1.0 | +| `longitudinal_velocity_noise_amp` | `double` | Target longitudinal velocity additional sine noise amplitude [m/s] | 0.01 | +| `longitudinal_velocity_noise_min_period` | `double` | Target longitudinal velocity additional sine noise minimum period [s] | 5.0 | +| `longitudinal_velocity_noise_max_period` | `double` | Target longitudinal velocity additional sine noise maximum period [s] | 20.0 | +| `acc_noise_amp` | `double` | Accel command additional sine noise amplitude [m/ss] | 0.01 | +| `acc_noise_min_period` | `double` | Accel command additional sine noise minimum period [s] | 5.0 | +| `acc_noise_max_period` | `double` | Accel command additional sine noise maximum period [s] | 20.0 | +| `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 | +| `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 | +| `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 | + +### Course-Specific Parameters + +Each trajectory has specific ROS 2 parameters. - `COURSE_NAME: eight_course` - -- `COURSE_NAME: u_shaped_return` - +| Name | Type | Description | Default value | +| :------------------ | :------- | :------------------------------------------- | :------------ | +| `velocity_on_curve` | `double` | Constant velocity on curve [m/s] | 4.5 | +| `smoothing_window` | `double` | Width of the window for trajectory smoothing | 400 | -- `COURSE_NAME: straight_line_positive` or `COURSE_NAME: straight_line_negative` - ( Both "straight_line_positive" and "straight_line_negative" represent straight line courses, but the direction of travel of the course is reversed.) - +- `COURSE_NAME: u_shaped_return` -## Parameter +| Name | Type | Description | Default value | +| :------------------ | :------- | :------------------------------- | :------------ | +| `velocity_on_curve` | `double` | Constant velocity on curve [m/s] | 4.5 | -ROS 2 params which are common in all nodes: +- `COURSE_NAME: straight_line_positive` or `COURSE_NAME: straight_line_negative` -| Name | Type | Description | Default value | -| :--------------------------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- | -| `LOAD_ROSBAG2_FILES` | `bool` | Flag that determines whether to load rosbag2 data or not | True | -| `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`] | `eight_course` | -| `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 | -| `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 10 | -| `NUM_BINS_A` | `int` | Number of bins of acceleration in heatmap | 10 | -| `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 | -| `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 | -| `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -1.0 | -| `STEER_MAX` | `double` | Maximum steer in heatmap [rad] | 1.0 | -| `A_MIN` | `double` | Minimum acceleration in heatmap [m/ss] | -1.0 | -| `A_MAX` | `double` | Maximum acceleration in heatmap [m/ss] | 1.0 | -| `COLLECTING_DATA_V_MIN` | `double` | Minimum velocity for data collection [m/s] | 0.0 | -| `COLLECTING_DATA_V_MAX` | `double` | Maximum velocity for data collection [m/s] | 11.5 | -| `COLLECTING_DATA_A_MIN` | `double` | Minimum velocity for data collection [m/ss] | 1.0 | -| `COLLECTING_DATA_A_MAX` | `double` | Maximum velocity for data collection [m/ss] | -1.0 | -| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 | -| `lateral_error_threshold` | `double` | Lateral error threshold where applying velocity limit [m/s] | 5.0 | -| `yaw_error_threshold` | `double` | Yaw error threshold where applying velocity limit [rad] | 0.50 | -| `velocity_limit_by_tracking_error` | `double` | Velocity limit applied when tracking error exceeds threshold [m/s] | 1.0 | -| `mov_ave_window` | `int` | Moving average smoothing window size | 100 | -| `target_longitudinal_velocity` | `double` | Target longitudinal velocity [m/s] | 6.0 | -| `longitudinal_velocity_noise_amp` | `double` | Target longitudinal velocity additional sine noise amplitude [m/s] | 0.01 | -| `longitudinal_velocity_noise_min_period` | `double` | Target longitudinal velocity additional sine noise minimum period [s] | 5.0 | -| `longitudinal_velocity_noise_max_period` | `double` | Target longitudinal velocity additional sine noise maximum period [s] | 20.0 | -| `pure_pursuit_type` | `string` | Pure pursuit type (`naive` or `linearized` steer control law ) | `linearized` | -| `wheel_base` | `double` | Wheel base [m] | 2.79 | -| `acc_kp` | `double` | Accel command proportional gain | 1.0 | -| `lookahead_time` | `double` | Pure pursuit lookahead time [s] | 2.0 | -| `min_lookahead` | `double` | Pure pursuit minimum lookahead length [m] | 2.0 | -| `linearized_pure_pursuit_steer_kp_param` | `double` | Linearized pure pursuit steering P gain parameter | 2.0 | -| `linearized_pure_pursuit_steer_kd_param` | `double` | Linearized pure pursuit steering D gain parameter | 2.0 | -| `stop_acc` | `double` | Accel command for stopping data collecting driving [m/ss] | -2.0 | -| `stop_jerk_lim` | `double` | Jerk limit for stopping data collecting driving [m/sss] | 5.0 | -| `lon_acc_lim` | `double` | Longitudinal acceleration limit [m/ss] | 5.0 | -| `lon_jerk_lim` | `double` | Longitudinal jerk limit [m/sss] | 5.0 | -| `steer_lim` | `double` | Steering angle limit [rad] | 1.0 | -| `steer_rate_lim` | `double` | Steering angle rate limit [rad/s] | 1.0 | -| `acc_noise_amp` | `double` | Accel command additional sine noise amplitude [m/ss] | 0.01 | -| `acc_noise_min_period` | `double` | Accel command additional sine noise minimum period [s] | 5.0 | -| `acc_noise_max_period` | `double` | Accel command additional sine noise maximum period [s] | 20.0 | -| `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 | -| `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 | -| `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 | +| Name | Type | Description | Default value | +| :------------------------- | :------- | :---------------------------------------------------- | :------------ | +| `stopping_buffer_distance` | `double` | The safety distance from end of the straight line [m] | 10.0 | + +- `COURSE_NAME: reversal_loop_circle` + +| Name | Type | Description | Default value | +| :-------------------- | :------- | :---------------------------------------------------------------------------------- | :------------ | +| `trajectory_radius` | `double` | Radius of the circle where trajectories are generated [m] | 35.0 | +| `enclosing_radius` | `double` | Radius of the circle enclosing the generated trajectories [m] | 40.0 | +| `look_ahead_distance` | `double` | The distance referenced ahead of the vehicle for collecting steering angle data [m] | 15.0 | + +- `COURSE_NAME: along_road` + +| Name | Type | Description | Default value | +| :-------------------------------- | :------- | :------------------------------------------------------------------ | :------------ | +| `velocity_on_curve` | `double` | Constant velocity on curve [m/s] | 3.5 | +| `stopping_buffer_distance` | `double` | The safety distance from end of the straight line [m] | 15.0 | +| `course_width` | `double` | The width of the trajectory [m] | 1.5 | +| `smoothing_window` | `double` | Width of the window for trajectory smoothing | 100 | +| `minimum_length_of_straight_line` | `double` | The minimum length of straight line for data collection [m] | 50.0 | +| `longitude` | `double` | The longitude of the origin specified when loading the map [degree] | 139.6503 | +| `latitude` | `double` | The latitude of the origin specified when loading the map [degree] | 35.6762 | diff --git a/control_data_collecting_tool/config/common_param.yaml b/control_data_collecting_tool/config/common_param.yaml new file mode 100644 index 00000000..e9221547 --- /dev/null +++ b/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.00 + lateral_error_threshold: 1.50 + 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 diff --git a/control_data_collecting_tool/config/course_param/along_road_param.yaml b/control_data_collecting_tool/config/course_param/along_road_param.yaml new file mode 100644 index 00000000..354b5dff --- /dev/null +++ b/control_data_collecting_tool/config/course_param/along_road_param.yaml @@ -0,0 +1,29 @@ +/**: + ros__parameters: + # Course Specific Parameters + velocity_on_curve: 3.5 + stopping_distance: 15.0 + course_width: 1.5 + smoothing_window: 100 + length_of_straight_line: 50.0 + longitude: 139.6503 + latitude: 35.6762 + + # 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 diff --git a/control_data_collecting_tool/config/course_param/eight_course_param.yaml b/control_data_collecting_tool/config/course_param/eight_course_param.yaml new file mode 100644 index 00000000..df6c9fb2 --- /dev/null +++ b/control_data_collecting_tool/config/course_param/eight_course_param.yaml @@ -0,0 +1,24 @@ +/**: + ros__parameters: + # Course Specific Parameters + velocity_on_curve: 4.5 + 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 diff --git a/control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml b/control_data_collecting_tool/config/course_param/reversal_loop_circle_param.yaml new file mode 100644 index 00000000..09621529 --- /dev/null +++ b/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 diff --git a/control_data_collecting_tool/config/course_param/straight_line_negative_param.yaml b/control_data_collecting_tool/config/course_param/straight_line_negative_param.yaml new file mode 100644 index 00000000..d443afd8 --- /dev/null +++ b/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 diff --git a/control_data_collecting_tool/config/course_param/straight_line_positive_param.yaml b/control_data_collecting_tool/config/course_param/straight_line_positive_param.yaml new file mode 100644 index 00000000..d443afd8 --- /dev/null +++ b/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 diff --git a/control_data_collecting_tool/config/course_param/u_shaped_return_param.yaml b/control_data_collecting_tool/config/course_param/u_shaped_return_param.yaml new file mode 100644 index 00000000..22d054a6 --- /dev/null +++ b/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 diff --git a/control_data_collecting_tool/config/param.yaml b/control_data_collecting_tool/config/param.yaml deleted file mode 100644 index e14b66a8..00000000 --- a/control_data_collecting_tool/config/param.yaml +++ /dev/null @@ -1,54 +0,0 @@ -/**: - 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 - - NUM_BINS_V: 10 - NUM_BINS_STEER: 10 - NUM_BINS_A: 10 - V_MIN: 0.0 - V_MAX: 11.5 - STEER_MIN: -1.0 - STEER_MAX: 1.0 - A_MIN: -1.0 - A_MAX: 1.0 - - COLLECTING_DATA_V_MIN: 0.0 - COLLECTING_DATA_V_MAX: 11.5 - COLLECTING_DATA_A_MIN: -1.0 - COLLECTING_DATA_A_MAX: 1.0 - - max_lateral_accel: 0.5 - lateral_error_threshold: 2.0 - yaw_error_threshold: 0.50 - velocity_limit_by_tracking_error: 1.0 - mov_ave_window: 100 - target_longitudinal_velocity: 6.0 - longitudinal_velocity_noise_amp: 0.01 - longitudinal_velocity_noise_min_period: 5.0 - longitudinal_velocity_noise_max_period: 20.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: 2.0 - lon_jerk_lim: 5.0 - steer_lim: 1.0 - steer_rate_lim: 1.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 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 index 475c7023..bfdcf910 100644 --- a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py +++ b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py @@ -19,31 +19,66 @@ 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 + map_path_arg = DeclareLaunchArgument( + "map_path", + description="Path to the map directory (optional; defaults to None if not provided).", + default_value="", # Default to None + ) + # 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( [ + 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=[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 +89,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], ), ] ) diff --git a/control_data_collecting_tool/plugins/plugin_description.xml b/control_data_collecting_tool/plugins/plugin_description.xml index f97c2f86..c4e85aa8 100644 --- a/control_data_collecting_tool/plugins/plugin_description.xml +++ b/control_data_collecting_tool/plugins/plugin_description.xml @@ -3,4 +3,8 @@ type="rviz_plugins::DataCollectingAreaSelectionTool" base_class_type="rviz_common::Tool"> + + diff --git a/control_data_collecting_tool/resource/DataCollectingAreaSelection.png b/control_data_collecting_tool/resource/DataCollectingAreaSelection.png new file mode 100644 index 00000000..a56cd8fc Binary files /dev/null and b/control_data_collecting_tool/resource/DataCollectingAreaSelection.png differ diff --git a/control_data_collecting_tool/resource/DataCollectingGoalPose.png b/control_data_collecting_tool/resource/DataCollectingGoalPose.png new file mode 100644 index 00000000..4af30412 Binary files /dev/null and b/control_data_collecting_tool/resource/DataCollectingGoalPose.png differ diff --git a/control_data_collecting_tool/resource/add_rviz_plugin.png b/control_data_collecting_tool/resource/add_rviz_plugin.png index fd739181..fc9c3d1c 100644 Binary files a/control_data_collecting_tool/resource/add_rviz_plugin.png and b/control_data_collecting_tool/resource/add_rviz_plugin.png differ diff --git a/control_data_collecting_tool/resource/along_load_plot.png b/control_data_collecting_tool/resource/along_load_plot.png new file mode 100644 index 00000000..a006ca52 Binary files /dev/null and b/control_data_collecting_tool/resource/along_load_plot.png differ diff --git a/control_data_collecting_tool/resource/along_road.png b/control_data_collecting_tool/resource/along_road.png new file mode 100644 index 00000000..fa18a673 Binary files /dev/null and b/control_data_collecting_tool/resource/along_road.png differ diff --git a/control_data_collecting_tool/resource/demo.gif b/control_data_collecting_tool/resource/demo.gif deleted file mode 100644 index 1ff9b4b6..00000000 Binary files a/control_data_collecting_tool/resource/demo.gif and /dev/null differ diff --git a/control_data_collecting_tool/resource/push_LOCAL.gif b/control_data_collecting_tool/resource/push_LOCAL.gif new file mode 100644 index 00000000..690003f3 Binary files /dev/null and b/control_data_collecting_tool/resource/push_LOCAL.gif differ diff --git a/control_data_collecting_tool/resource/push_LOCAL.png b/control_data_collecting_tool/resource/push_LOCAL.png index a20f8836..ee6e689b 100644 Binary files a/control_data_collecting_tool/resource/push_LOCAL.png and b/control_data_collecting_tool/resource/push_LOCAL.png differ diff --git a/control_data_collecting_tool/resource/reversal_loop_circle.png b/control_data_collecting_tool/resource/reversal_loop_circle.png new file mode 100644 index 00000000..2ad4cab0 Binary files /dev/null and b/control_data_collecting_tool/resource/reversal_loop_circle.png differ diff --git a/control_data_collecting_tool/resource/select_area.gif b/control_data_collecting_tool/resource/select_area.gif index 051eeb0a..9fc40b00 100644 Binary files a/control_data_collecting_tool/resource/select_area.gif and b/control_data_collecting_tool/resource/select_area.gif differ diff --git a/control_data_collecting_tool/resource/select_tool.png b/control_data_collecting_tool/resource/select_tool.png deleted file mode 100644 index 3fb812ce..00000000 Binary files a/control_data_collecting_tool/resource/select_tool.png and /dev/null differ diff --git a/control_data_collecting_tool/resource/set_trajectory_along_road.gif b/control_data_collecting_tool/resource/set_trajectory_along_road.gif new file mode 100644 index 00000000..29a0aeb2 Binary files /dev/null and b/control_data_collecting_tool/resource/set_trajectory_along_road.gif differ diff --git a/control_data_collecting_tool/scripts/courses/along_road.py b/control_data_collecting_tool/scripts/courses/along_road.py new file mode 100644 index 00000000..eed4f7b6 --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/along_road.py @@ -0,0 +1,343 @@ +#!/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. + +from courses.base_course import Base_Course +from courses.lanelet import LaneletMapHandler +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor +from scipy.interpolate import interp1d as interpolation_1d + + +def resample_curve(x, y, step_size): + # Calculate the distance between each point and find the cumulative distance + dx = x[:-1] - x[1:] + dy = y[:-1] - y[1:] + distances = np.sqrt(dx**2 + dy**2) + cumulative_distances = np.concatenate([[0], np.cumsum(distances)]) + + num_samples = int(cumulative_distances[-1] / step_size) + # Calculate new distances for resampling at equal intervals along the cumulative distance + new_distances = np.linspace(0, cumulative_distances[-1], num_samples) + + # Interpolate x and y based on the distances, then resample + x_interpolation = interpolation_1d(cumulative_distances, x, kind="linear") + y_interpolation = interpolation_1d(cumulative_distances, y, kind="linear") + new_x = x_interpolation(new_distances) + new_y = y_interpolation(new_distances) + + # Return the resampled points along the curve + return new_x, new_y + + +def declare_along_road_params(node): + # Declare the parameters for the along_road course + node.declare_parameter( + "smoothing_window", + 100, + ParameterDescriptor(description="Width of the window for trajectory smoothing"), + ) + node.declare_parameter( + "velocity_on_curve", + 3.5, + ParameterDescriptor(description="Constant velocity on curve [m/s]"), + ) + node.declare_parameter( + "stopping_distance", + 15.0, + ParameterDescriptor(description="The safety distance from end of the straight line [m]"), + ) + node.declare_parameter( + "course_width", + 1.5, + ParameterDescriptor(description="The width of the trajectory [m]"), + ) + node.declare_parameter( + "minimum_length_of_straight_line", + 50.0, + ParameterDescriptor( + description="The minimum length of straight line for data collection [m]" + ), + ) + node.declare_parameter( + "longitude", + 139.6503, + ParameterDescriptor( + description="The longitude of the origin specified when loading the map" + ), + ) + node.declare_parameter( + "latitude", + 35.6762, + ParameterDescriptor( + description="The latitude of the origin specified when loading the map" + ), + ) + node.declare_parameter( + "map_path", + "", + descriptor=ParameterDescriptor( + description="Path to the map directory", dynamic_typing=True + ), + ) + + +class Along_Road(Base_Course): + def __init__(self, step: float, param_dict): + super().__init__(step, param_dict) + self.closed = False + + longitude = param_dict["longitude"] + latitude = param_dict["latitude"] + map_path = param_dict["map_path"] + "/lanelet2_map.osm" + self.handler = LaneletMapHandler(map_path, longitude, latitude) + + self.window_size = param_dict["smoothing_window"] + + self.set_target_velocity_on_straight_line = False + self.target_vel_on_straight_line = 0.0 + self.target_acc_on_straight_line = 0.0 + self.vel_idx, self.acc_idx = 0, 0 + self.previous_part = "curve" + + self.deceleration_rate = 1.0 + + self.sine_period_for_velocity = 7.5 + self.velocity_on_curve = param_dict["velocity_on_curve"] + self.stopping_distance = param_dict["stopping_distance"] + + self.course_width = param_dict["course_width"] + self.minimum_length_of_straight_line = param_dict["minimum_length_of_straight_line"] + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point, + goal_point, + curvature_threshold=1e-2, + ): + # Set the minimum length of straight line for data collection + straight_segment_threshold = self.minimum_length_of_straight_line + + # Get the shortest path between ego_point and goal_point + x, y = self.handler.get_shortest_path(ego_point, goal_point) + if x is None or y is None: # Exit if no valid path is found + return None + + # Resample the trajectory to ensure uniform step intervals + x, y = resample_curve(x, y, self.step) + + # Store the trajectory points as a 2D array of [x, y] + self.trajectory_points = np.array([x, y]).T + + # Initialize segment classification (straight or curve) and achievement rates + self.parts = [] + self.achievement_rates = np.zeros(len(x)) + + # Compute the yaw (heading angle) of the trajectory + dx = (x[1:] - x[:-1]) / self.step + dy = (y[1:] - y[:-1]) / self.step + self.yaw = np.arctan2(dy, dx) # Calculate the heading angles + self.yaw = np.array( + self.yaw.tolist() + [self.yaw[-1]] + ) # Extend to match the trajectory length + + ddx = (dx[1:] - dx[:-1]) / self.step + ddy = (dy[1:] - dy[:-1]) / self.step + self.curvature = ( + 1e-9 + abs(ddx * dy[:-1] - ddy * dx[:-1]) / (dx[:-1] ** 2 + dy[:-1] ** 2 + 1e-9) ** 1.5 + ) + self.curvature = np.array( + self.curvature.tolist() + [self.curvature[-1], self.curvature[-1]] + ) + + # Prepare for trajectory smoothing + window_size = self.window_size # Size of the moving average window + # Extend the trajectory at both ends to apply smoothing + augmented_x = np.concatenate( + (x[0] * np.ones(window_size // 2), x, x[-1] * np.ones(window_size // 2 - 1)) + ) + augmented_y = np.concatenate( + (y[0] * np.ones(window_size // 2), y, y[-1] * np.ones(window_size // 2 - 1)) + ) + + # Compute smoothed trajectory using a moving average + x_smoothed = np.convolve(augmented_x, np.ones(window_size) / window_size, mode="valid") + y_smoothed = np.convolve(augmented_y, np.ones(window_size) / window_size, mode="valid") + + # Compute first derivatives (velocity) and second derivatives (acceleration) of the smoothed trajectory + dx_smoothed = (x_smoothed[1:] - x_smoothed[:-1]) / self.step + dy_smoothed = (y_smoothed[1:] - y_smoothed[:-1]) / self.step + ddx_smoothed = (dx_smoothed[1:] - dx_smoothed[:-1]) / self.step + ddy_smoothed = (dy_smoothed[1:] - dy_smoothed[:-1]) / self.step + + # Calculate the curvature of the smoothed trajectory + smoothed_curvature = ( + 1e-9 + + abs(ddx_smoothed * dy_smoothed[:-1] - ddy_smoothed * dx_smoothed[:-1]) + / (dx_smoothed[:-1] ** 2 + dy_smoothed[:-1] ** 2 + 1e-9) ** 1.5 + ) + # Extend the curvature array to match the trajectory length + smoothed_curvature = np.array( + smoothed_curvature.tolist() + [smoothed_curvature[-1], smoothed_curvature[-1]] + ) + + # Classify each point in the trajectory as "straight" or "curve" based on curvature + for i in range(len(smoothed_curvature)): + if smoothed_curvature[i] < curvature_threshold: + self.parts.append("straight") + else: + self.parts.append("curve") + + # Identify start and end indices of straight segments + previous_part = "curve" + start_index = [] + end_index = [] + for i, part in enumerate(self.parts): + current_part = part + + # Detect the transition from "curve" to "straight" + if previous_part == "curve" and current_part == "straight": + start_index.append(i) + + # Detect the transition from "straight" to "curve" + if previous_part == "straight" and current_part == "curve": + end_index.append(i - 1) + + # Handle the case where the last segment ends as "straight" + if i == len(self.parts) - 1 and len(start_index) > len(end_index): + end_index.append(i - 1) + + previous_part = current_part + + # Assign achievement rates to sufficiently long straight segments + for i in range(len(start_index)): + st = start_index[i] + ed = end_index[i] + + # Only assign achievement rates to straight segments longer than the threshold + if (ed - st) * self.step > straight_segment_threshold: + self.achievement_rates[st : ed + 1] = np.linspace(1e-4, 1.0, ed - st + 1) + + # Update segment classification based on achievement rates + for i in range(len(self.parts)): + if self.achievement_rates[i] > 0.0: + self.parts[i] = "straight" + else: + self.parts[i] = "curve" + + # Plot the trajectory on the map to check which part is classified as straight or curve + self.handler.plot_trajectory_on_map(self.trajectory_points, self.parts) + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + part = self.parts[nearestIndex] + achievement_rate = self.achievement_rates[nearestIndex] + acc_kp_of_pure_pursuit = self.params.acc_kp + + # Check and update target velocity on straight line + if ( + (part == "straight" and self.previous_part == "curve") + or (part == "straight" and achievement_rate < 0.05) + ) and not self.set_target_velocity_on_straight_line: + self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( + collected_data_counts_of_vel_acc + ) + self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] + self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] + + i = 0 + while self.parts[i + nearestIndex] == "straight": + i += 1 + + distance = i * self.step + stop_distance = self.target_vel_on_straight_line**2 / (2 * self.params.a_max) + self.deceleration_rate = 1.0 - 20.0 / distance - stop_distance / distance + self.set_target_velocity_on_straight_line = True + + # Reset target velocity on line if entering a curve + if part == "curve": + self.set_target_velocity_on_straight_line = False + + self.previous_part = part + + # Calculate sine wave and apply to velocity + T = self.sine_period_for_velocity + sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + + if current_vel > self.target_vel_on_straight_line: + target_vel = self.target_vel_on_straight_line + sine + elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( + self.target_acc_on_straight_line + ): + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.5 * sine + ) + else: + target_vel = current_vel + abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + + # Adjust for deceleration based on achievement rate + if self.deceleration_rate - 0.05 <= achievement_rate < self.deceleration_rate: + target_vel = current_vel - abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + elif self.deceleration_rate <= achievement_rate: + target_vel = max( + current_vel - self.params.a_max / acc_kp_of_pure_pursuit * (1.0 + 0.5 * sine), + self.velocity_on_curve, + ) + + # Handle special conditions for curves or trajectory end + if part == "curve" or nearestIndex > len(self.trajectory_points) - int( + 2.0 * self.stopping_distance / self.step + ): + target_vel = self.velocity_on_curve + + if nearestIndex > len(self.trajectory_points) - int(self.stopping_distance / self.step): + target_vel = 0.0 + + return target_vel + + def return_trajectory_points(self, yaw, translation): + # no coordinate transformation is needed + return self.trajectory_points, self.yaw, self.curvature, self.parts, self.achievement_rates + + def get_boundary_points(self): + if self.trajectory_points is None or self.yaw is None: + return None + + upper_boundary_points = [] + lower_boundary_points = [] + + for point, yaw in zip(self.trajectory_points, self.yaw): + normal = self.course_width * np.array( + [np.cos(yaw + np.pi / 2.0), np.sin(yaw + np.pi / 2.0)] + ) + upper_boundary_points.append(point + normal) + lower_boundary_points.append(point - normal) + + lower_boundary_points.reverse() + + self.boundary_points = np.array(upper_boundary_points + lower_boundary_points) diff --git a/control_data_collecting_tool/scripts/courses/base_course.py b/control_data_collecting_tool/scripts/courses/base_course.py new file mode 100644 index 00000000..3866aee4 --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/base_course.py @@ -0,0 +1,137 @@ +#!/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 +from params import Params + + +class Base_Course: + def __init__(self, step: float, param_dict): + self.step = step + self.trajectory_points = None + self.yaw = None + self.parts = None + self.curvature = None + self.achievement_rates = None + + self.boundary_points = None + self.boundary_yaw = None + + self.closed = True + + self.A = np.array([0.0, 0.0]) + self.B = np.array([0.0, 0.0]) + self.C = np.array([0.0, 0.0]) + self.D = np.array([0.0, 0.0]) + + self.params = Params(param_dict) + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point=np.array([0.0, 0.0]), + goal_point=np.array([0.0, 0.0]), + ): + pass + + def choose_target_velocity_acc(self, collected_data_counts_of_vel_acc): + min_num_data = 1e12 + min_data_num_margin = 20 + min_index_list = [] + + for i in range(self.params.collecting_data_min_n_v, self.params.collecting_data_max_n_v): + for j in range( + self.params.collecting_data_min_n_a, self.params.collecting_data_max_n_a + ): + if min_num_data - min_data_num_margin > collected_data_counts_of_vel_acc[i, j]: + min_num_data = collected_data_counts_of_vel_acc[i, j] + min_index_list.clear() + min_index_list.append((j, i)) + + elif min_num_data + min_data_num_margin > collected_data_counts_of_vel_acc[i, j]: + min_index_list.append((j, i)) + + return min_index_list[np.random.randint(0, len(min_index_list))] + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + pass + + def set_vertices(self, A, B, C, D): + self.A = A + self.B = B + self.C = C + self.D = D + + def get_boundary_points(self): + pass + + def check_in_boundary(self, current_position): + x, y = current_position[0], current_position[1] + polygon = self.boundary_points + wn = 0 + + for i in range(len(polygon)): + x1, y1 = polygon[i][0], polygon[i][1] + x2, y2 = polygon[(i + 1) % len(polygon)][0], polygon[(i + 1) % len(polygon)][1] + + # Calculate if the point is to the left of the edge + is_left = (x2 - x1) * (y - y1) - (x - x1) * (y2 - y1) > 0 + + if y1 <= y < y2 and is_left: # Upward crossing + wn += 1 + elif y2 <= y < y1 and not is_left: # Downward crossing + wn -= 1 + + return wn != 0 + + def return_trajectory_points(self, yaw_offset, rectangle_center_position): + rot_matrix = np.array( + [ + [np.cos(yaw_offset), -np.sin(yaw_offset)], + [np.sin(yaw_offset), np.cos(yaw_offset)], + ] + ) + + trajectory_position_data = (rot_matrix @ self.trajectory_points.T).T + trajectory_position_data += rectangle_center_position + trajectory_yaw_data = self.yaw + yaw_offset + + return ( + trajectory_position_data, + trajectory_yaw_data, + self.curvature, + self.parts, + self.achievement_rates, + ) + + def update_trajectory_points( + self, + nearestIndex, + yaw_offset, + rectangle_center_position, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + return nearestIndex, *self.return_trajectory_points(yaw_offset, rectangle_center_position) diff --git a/control_data_collecting_tool/scripts/courses/figure_eight.py b/control_data_collecting_tool/scripts/courses/figure_eight.py new file mode 100644 index 00000000..1bb20b52 --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/figure_eight.py @@ -0,0 +1,264 @@ +#!/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. + +from courses.base_course import Base_Course +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor + + +def computeTriangleArea(A, B, C): + return 0.5 * abs(np.cross(B - A, C - A)) + + +def declare_figure_eight_params(node): + node.declare_parameter( + "velocity_on_curve", + 4.5, + ParameterDescriptor(description="Constant velocity on curve [m/s] "), + ) + node.declare_parameter( + "smoothing_window", + 400, + ParameterDescriptor(description="Width of the window for trajectory smoothing"), + ) + + +class Figure_Eight(Base_Course): + def __init__(self, step: float, param_dict): + super().__init__(step, param_dict) + + self.window_size = param_dict["smoothing_window"] + self.set_target_velocity_on_straight_line = False + self.target_vel_on_straight_line = 6.0 + self.target_acc_on_straight_line = 0.0 + self.vel_idx, self.acc_idx = 0, 0 + self.previous_part = "curve" + + self.deceleration_rate = 1.0 + + self.sine_period_for_velocity = 7.5 + self.velocity_on_curve = param_dict["velocity_on_curve"] + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point=np.array([0.0, 0.0]), + goal_point=np.array([0.0, 0.0]), + ): + a = short_side_length + b = long_side_length + + C = [-(b / 2 - (1.0 - np.sqrt(3) / 2) * a), -a / 2] + D = [(b / 2 - (1.0 - np.sqrt(3) / 2) * a), -a / 2] + + R = a # radius of the circle + OL = [-b / 2 + a, 0] # center of the left circle + OR = [b / 2 - a, 0] # center of the right circle + OB = np.sqrt( + (b / 2 + (1.0 - np.sqrt(3) / 2) * a) ** 2 + (a / 2) ** 2 + ) # half length of the straight trajectory + AD = 2 * OB + BD = 2 * np.pi * R / 6 # the length of arc BD + AC = BD + CO = OB + + total_distance = 4 * OB + 2 * BD + + t_array = np.arange(start=0.0, stop=total_distance, step=self.step).astype("float") + x = np.array([0.0 for i in range(len(t_array.copy()))]) + y = np.array([0.0 for i in range(len(t_array.copy()))]) + self.yaw = t_array.copy() + self.curvature = t_array.copy() + self.achievement_rates = t_array.copy() + self.parts = ["part" for _ in range(len(t_array.copy()))] + i_end = t_array.shape[0] + + for i, t in enumerate(t_array): + if t > OB + BD + AD + AC + CO: + i_end = i + break + + if 0 <= t and t <= OB: + x[i] = (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t / OB + y[i] = a * t / (2 * OB) + self.parts[i] = "straight_positive" + self.achievement_rates[i] = t / (2 * OB) + 0.5 + + if OB <= t and t <= OB + BD: + t1 = t - OB + t1_rad = t1 / R + x[i] = OR[0] + R * np.cos(np.pi / 6 - t1_rad) + y[i] = OR[1] + R * np.sin(np.pi / 6 - t1_rad) + self.parts[i] = "curve" + self.achievement_rates[i] = t1 / BD + + if OB + BD <= t and t <= OB + BD + AD: + t2 = t - (OB + BD) + x[i] = D[0] - (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t2 / OB + y[i] = D[1] + a * t2 / (2 * OB) + self.parts[i] = "straight" + self.achievement_rates[i] = t2 / (2 * OB) + + if OB + BD + AD <= t and t <= OB + BD + AD + AC: + t3 = t - (OB + BD + AD) + t3_rad = t3 / R + x[i] = OL[0] - R * np.cos(-np.pi / 6 + t3_rad) + y[i] = OL[1] - R * np.sin(-np.pi / 6 + t3_rad) + self.parts[i] = "curve" + self.achievement_rates[i] = t3 / BD + + if OB + BD + AD + AC <= t and t <= OB + BD + AD + AC + CO: + t4 = t - (OB + BD + AD + AC) + x[i] = C[0] + (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t4 / OB + y[i] = C[1] + a * t4 / (2 * OB) + self.parts[i] = "straight_positive" + self.achievement_rates[i] = t4 / (2 * OB) + + # drop rest + x = x[:i_end] + y = y[:i_end] + self.parts = self.parts[:i_end] + self.achievement_rates = self.achievement_rates[:i_end] + + x = np.concatenate((x, x)) + y = np.concatenate((y, y)) + + self.parts = np.concatenate((self.parts, self.parts)) + self.achievement_rates = np.concatenate((self.achievement_rates, self.achievement_rates)) + window_size = self.window_size + x = np.concatenate((x[-window_size // 2 :], x, x[: window_size // 2])) + y = np.concatenate((y[-window_size // 2 :], y, y[: window_size // 2])) + + x_smoothed = np.convolve(x, np.ones(window_size) / window_size, mode="valid")[ + window_size // 2 : -window_size // 2 + ] + y_smoothed = np.convolve(y, np.ones(window_size) / window_size, mode="valid")[ + window_size // 2 : -window_size // 2 + ] + self.trajectory_points = 1.0 * np.array([x_smoothed, y_smoothed]).T + + dx = (x_smoothed[1:] - x_smoothed[:-1]) / self.step + dy = (y_smoothed[1:] - y_smoothed[:-1]) / self.step + + ddx = (dx[1:] - dx[:-1]) / self.step + ddy = (dy[1:] - dy[:-1]) / self.step + + self.yaw = np.arctan2(dy, dx) + self.yaw = np.array(self.yaw.tolist() + [self.yaw[-1]]) + + self.curvature = ( + 1e-9 + abs(ddx * dy[:-1] - ddy * dx[:-1]) / (dx[:-1] ** 2 + dy[:-1] ** 2) ** 1.5 + ) + self.curvature = np.array( + self.curvature.tolist() + [self.curvature[-2], self.curvature[-1]] + ) + + return self.trajectory_points, self.yaw, self.curvature, self.parts, self.achievement_rates + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + part = self.parts[nearestIndex] + achievement_rate = self.achievement_rates[nearestIndex] + acc_kp_of_pure_pursuit = self.params.acc_kp + + # Check and update target velocity on straight line + if ( + (part == "straight" and self.previous_part == "curve") + or (part == "straight" and achievement_rate < 0.05) + ) and not self.set_target_velocity_on_straight_line: + self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( + collected_data_counts_of_vel_acc + ) + self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] + self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] + + i = 0 + while self.parts[i + nearestIndex] == "straight": + i += 1 + + distance = i * self.step + stop_distance = self.target_vel_on_straight_line**2 / (2 * self.params.a_max) + self.deceleration_rate = 1.0 - stop_distance / distance + self.set_target_velocity_on_straight_line = True + + # Reset target velocity on line if entering a curve + if part == "curve": + self.set_target_velocity_on_straight_line = False + + self.previous_part = part + + # Calculate sine wave and apply to velocity + T = self.sine_period_for_velocity + sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + + if current_vel > self.target_vel_on_straight_line: + target_vel = self.target_vel_on_straight_line + sine + 1.5 * sine - 1.0 + target_vel = max(target_vel, 0.05) + elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( + self.target_acc_on_straight_line + ): + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.5 * sine + ) + else: + target_vel = current_vel + abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + + # Adjust for deceleration based on achievement rate + if self.deceleration_rate - 0.05 <= achievement_rate < self.deceleration_rate: + target_vel = current_vel - abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + elif self.deceleration_rate <= achievement_rate: + target_vel = max( + current_vel - self.params.a_max / acc_kp_of_pure_pursuit * (1.0 + 0.5 * sine), + self.velocity_on_curve, + ) + + # Handle special conditions for curves or trajectory end + if part == "curve": + target_vel = self.velocity_on_curve + + return target_vel + + def get_boundary_points(self): + self.boundary_points = np.vstack((self.A, self.B, self.C, self.D)) + + def check_in_boundary(self, current_position): + P = current_position[0:2] + + area_ABCD = computeTriangleArea(self.A, self.B, self.C) + computeTriangleArea( + self.C, self.D, self.A + ) + + area_PAB = computeTriangleArea(P, self.A, self.B) + area_PBC = computeTriangleArea(P, self.B, self.C) + area_PCD = computeTriangleArea(P, self.C, self.D) + area_PDA = computeTriangleArea(P, self.D, self.A) + + if area_PAB + area_PBC + area_PCD + area_PDA > area_ABCD * 1.001: + return False + else: + return True diff --git a/control_data_collecting_tool/scripts/courses/lanelet.py b/control_data_collecting_tool/scripts/courses/lanelet.py new file mode 100644 index 00000000..87687671 --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/lanelet.py @@ -0,0 +1,293 @@ +#!/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 lanelet2 +from lanelet2.core import BasicPoint2d +from lanelet2.core import BoundingBox2d +from lanelet2.io import Origin +from lanelet2.io import load +from lanelet2.routing import RoutingGraph +from lanelet2_extension_python.projection import MGRSProjector +import matplotlib.pyplot as plt +import numpy as np +from scipy.interpolate import interp1d as interpolation_1d + + +class LaneletUtils: + @staticmethod + def search_nearest_lanelet(point2d, handler, search_radius=5.0): + # Set a search radius to create a bounding box around the point + radius = BasicPoint2d(search_radius, search_radius) + bb = BoundingBox2d(point2d - radius, point2d + radius) + # Search for lanelets within the bounding box + lanelets_at_p = handler.lanelet_map.laneletLayer.search(bb) + # Find the nearest lanelet to the specified point + nearest_lanelet = None + min_distance = float("inf") + for lanelet_at_p in lanelets_at_p: + closest_idx = LaneletUtils.closest_segment(lanelet_at_p.centerline, point2d) + center_point = lanelet_at_p.centerline[closest_idx] + distance = np.linalg.norm([point2d.x - center_point.x, point2d.y - center_point.y]) + if distance < min_distance: + min_distance = distance + nearest_lanelet = lanelet_at_p + + return nearest_lanelet + + @staticmethod + def closest_segment(lineString, pointToProject2d): + # Find the closest segment in the lineString to the specified point + min_dist = float("inf") + min_idx = None + for idx in range(len(lineString) - 1): + point = lineString[idx] + point_suc = lineString[idx + 1] + # Calculate the perpendicular distance from the point to the line segment + dist = LaneletUtils.approximate_distance( + point, point_suc, pointToProject2d + ) # In trajectory generation, approximate_distance performs better than perpendicular_distance. + # dist = LaneletUtils.perpendicular_distance(point, point_suc, pointToProject2d) + if dist < min_dist: + min_dist = dist + min_idx = idx + return min_idx + + @staticmethod + def approximate_distance(point1, point2, point): + # Calculate the perpendicular distance from a point to a line segment + a = np.linalg.norm([point1.x - point.x, point1.y - point.y]) + b = np.linalg.norm([point2.x - point.x, point2.y - point.y]) + return np.min([a, b]) + + """ + @staticmethod + def perpendicular_distance(point1, point2, point): + # Calculate the perpendicular distance from a point to a line segment + a = np.array([point1.x - point2.x, point1.y - point2.y]) + b = np.array([point1.x - point.x, point1.y - point.y]) + return np.linalg.norm(np.cross(a, b) / np.linalg.norm(a)) + """ + + @staticmethod + def interpolate_two_lines(oneLine, anotherLine): + # Create interpolation parameters based on the length of each line + t_one = np.linspace(0.0, 1.0, len(oneLine)) + t_another = np.linspace(0.0, 1.0, len(anotherLine)) + + # Set up linear interpolation functions for both lines + linear_interpolation_one = interpolation_1d( + t_one, oneLine, axis=0, kind="linear", fill_value="extrapolate" + ) + linear_interpolation_another = interpolation_1d( + t_another, anotherLine, axis=0, kind="linear", fill_value="extrapolate" + ) + + # Define a common interpolation parameter + t_interpolation = np.linspace(0.0, 1.0, max(len(oneLine), len(anotherLine))) + + # Perform interpolation between the two lines + interpolated_points = np.array( + [(1.0 - t_interpolation).tolist()] + ).T * linear_interpolation_one(t_interpolation) + np.array( + [t_interpolation.tolist()] + ).T * linear_interpolation_another( + t_interpolation + ) + + return interpolated_points + + +class LaneletMapHandler: + def __init__(self, map_path, longitude, latitude): + # Initialize the map projector and load the lanelet map + projector = MGRSProjector(Origin(latitude, longitude)) + + # to do : error handling when loading map if necessary + self.lanelet_map = load(map_path, projector) + + # Initialize traffic rules and routing graph + self.traffic_rules = lanelet2.traffic_rules.create( + lanelet2.traffic_rules.Locations.Germany, + lanelet2.traffic_rules.Participants.Vehicle, + ) + self.routing_graph = RoutingGraph(self.lanelet_map, self.traffic_rules) + + # Store the segmented shortest route + self.shortest_segmented_route = [] + + def get_shortest_path(self, ego_point, goal_point): + # ego_point = goal_point + # goal_point = goal_point + + # Find the nearest lanelet and index to start and goal points + ego_lanelet, idx_ego_lanelet = self.find_nearest_lanelet_with_index(ego_point) + goal_lanelet, idx_goal_lanelet = self.find_nearest_lanelet_with_index(goal_point) + + if ego_lanelet is None or goal_lanelet is None: + return None, None + + # Check if start and goal are in the same lanelet + if ego_lanelet == goal_lanelet: + ego_lanelet = self.get_following_lanelet(ego_lanelet) + if ego_lanelet is None: + return None, None # Return None if no path can be found + + # Get the shortest path between the two lanelets + shortest_path = self.routing_graph.getRoute(ego_lanelet, goal_lanelet).shortestPath() + + if shortest_path is None: + return None, None + + # Segment the path and store the result + self.shortest_segmented_route = self.segment_path( + shortest_path, idx_ego_lanelet, idx_goal_lanelet + ) + + x = [] + y = [] + # Store the x and y coordinates + for segment in self.shortest_segmented_route: + x += [point[0] for point in segment] + y += [point[1] for point in segment] + + return np.array(x), np.array(y) + + def segment_path(self, shortest_path, start_idx, end_idx): + # Segment the shortest path based on start and goal indexes + lanelet_center_cache = {} + segmented_route = [] + + i = 0 + while i < len(shortest_path): + lanelet = shortest_path[i] + # Cache centerline for efficiency + center_line = lanelet_center_cache.setdefault( + lanelet.id, self.get_lanelet_centerline(lanelet) + ) + + # Check for lane changes and interpolate if necessary + if i < len(shortest_path) - 1: + center_line, plus_lane_idx = self.check_lane_change( + lanelet, shortest_path[i + 1], center_line + ) + i += plus_lane_idx + + # Adjust segments based on path position + if i == 0: + segment = center_line[start_idx:] + elif i == len(shortest_path) - 1: + segment = center_line[:end_idx] + else: + segment = center_line + segmented_route.append(segment) + + i += 1 + return segmented_route + + def find_nearest_lanelet_with_index(self, point): + # Find the nearest lanelet and the closest segment index + point_2D = BasicPoint2d(point[0], point[1]) + lanelet = LaneletUtils.search_nearest_lanelet(point_2D, self) + + if lanelet is None: + return None, None + idx = LaneletUtils.closest_segment(lanelet.centerline, point_2D) + return lanelet, idx + + def get_following_lanelet(self, lanelet): + # Retrieve the next lanelet if it exists + following_relations = self.routing_graph.followingRelations(lanelet) + return following_relations[0].lanelet if following_relations else None + + def get_lanelet_centerline(self, lanelet): + # Return the centerline points of a lanelet + return [[point.x, point.y] for point in lanelet.centerline] + + def check_lane_change(self, current_lanelet, next_lanelet, center_line): + # Check if a lane change to the next lanelet is possible + for adjacent in self.routing_graph.rightRelations( + current_lanelet + ) + self.routing_graph.leftRelations(current_lanelet): + if adjacent.lanelet == next_lanelet: + interpolated_line = LaneletUtils.interpolate_two_lines( + center_line, self.get_lanelet_centerline(next_lanelet) + ) + return interpolated_line, 1 + return center_line, 0 + + def plot_trajectory_on_map(self, trajectory=None, trajectory_parts=None): + plt.close() + + # Use interactive mode for plotting + plt.ion() + + # Set up the plot for displaying the lanelet map + plt.figure(figsize=(8, 8)) + plt.xlabel("X (m)") + plt.ylabel("Y (m)") + plt.title("Lanelet2 Map Boundary Lines") + + # Plot boundaries of each lanelet + for lanelet in self.lanelet_map.laneletLayer: + plt.plot( + [p.x for p in lanelet.leftBound], [p.y for p in lanelet.leftBound], color="gray" + ) + plt.plot( + [p.x for p in lanelet.rightBound], [p.y for p in lanelet.rightBound], color="gray" + ) + + # Plot the trajectory if provided + if trajectory is not None and trajectory_parts is None: + trajectory_len = len(trajectory) + plt.plot( + [trajectory[i, 0] for i in range(trajectory_len)], + [trajectory[i, 1] for i in range(trajectory_len)], + linestyle="--", + linewidth=2, + ) + + elif trajectory is not None and trajectory_parts is not None: + trajectory_len = len(trajectory) + previous_part = trajectory_parts[0] + + trajectory_x = [] + trajectory_y = [] + + for i in range(trajectory_len): + if trajectory_parts[i] is not previous_part or i == trajectory_len - 1: + if previous_part == "curve": + label = "velocity = const (velocity_on_curve)" + elif previous_part == "straight": + label = "Data collection is conducted" + + plt.plot( + trajectory_x, + trajectory_y, + linestyle="--", + linewidth=2, + label=label, + ) + + trajectory_x = [] + trajectory_y = [] + + trajectory_x.append(trajectory[i, 0]) + trajectory_y.append(trajectory[i, 1]) + previous_part = trajectory_parts[i] + + plt.legend() + plt.show() + plt.pause(1.0) diff --git a/control_data_collecting_tool/scripts/courses/load_course.py b/control_data_collecting_tool/scripts/courses/load_course.py new file mode 100644 index 00000000..11e15b3d --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/load_course.py @@ -0,0 +1,64 @@ +#!/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. + + +from courses.along_road import Along_Road +from courses.along_road import declare_along_road_params +from courses.figure_eight import Figure_Eight +from courses.figure_eight import declare_figure_eight_params +from courses.reversal_loop_circle import Reversal_Loop_Circle +from courses.reversal_loop_circle import declare_reversal_loop_circle_params +from courses.straight_line_negative import Straight_Line_Negative +from courses.straight_line_negative import declare_straight_line_negative_params +from courses.straight_line_positive import Straight_Line_Positive +from courses.straight_line_positive import declare_straight_line_positive_params +from courses.u_shaped import U_Shaped +from courses.u_shaped import declare_u_shaped_return_params + + +def declare_course_params(course_name, node): + # Declare the course parameters based on the course name + if course_name == "eight_course": + declare_figure_eight_params(node) + elif course_name == "straight_line_positive": + declare_straight_line_positive_params(node) + elif course_name == "straight_line_negative": + declare_straight_line_negative_params(node) + elif course_name == "u_shaped_return": + declare_u_shaped_return_params(node) + elif course_name == "reversal_loop_circle": + declare_reversal_loop_circle_params(node) + elif course_name == "along_road": + declare_along_road_params(node) + + +def load_course(course_name, step_size, params_dict): + # Load the course based on the course name + if course_name == "eight_course": + course = Figure_Eight(step_size, params_dict) + elif course_name == "straight_line_positive": + course = Straight_Line_Positive(step_size, params_dict) + elif course_name == "straight_line_negative": + course = Straight_Line_Negative(step_size, params_dict) + elif course_name == "u_shaped_return": + course = U_Shaped(step_size, params_dict) + elif course_name == "reversal_loop_circle": + course = Reversal_Loop_Circle(step_size, params_dict) + elif course_name == "along_road": + course = Along_Road(step_size, params_dict) + + # Return the course + return course diff --git a/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py b/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py new file mode 100644 index 00000000..3650e3a5 --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/reversal_loop_circle.py @@ -0,0 +1,1236 @@ +#!/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. + +from collections import deque +import copy +import math +import random +import statistics + +from courses.base_course import Base_Course +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor + + +def safe_acos(value): + return math.acos(max(-1, min(1, value))) + + +def calc_tangent_contact_points(x, y, r, a, b, c): + # Check if the line has a non-zero b to solve for y = (c - ax) / b + if b != 0: + # Define coefficients for the quadratic equation A*x^2 + B*x + C = 0 + A = 1 + (a / b) ** 2 + B = -2 * (x + (a * (c - b * y)) / b**2) + C = x**2 + ((c - b * y) / b) ** 2 - r**2 + + # Calculate the discriminant + abs_discriminant = abs(B**2 - 4 * A * C) + + # Calculate the two x coordinates of the tangent points + sqrt_discriminant = math.sqrt(abs_discriminant) + x_contact1 = (-B + sqrt_discriminant) / (2 * A) + x_contact2 = (-B - sqrt_discriminant) / (2 * A) + + # Corresponding y coordinates for each x + y_contact1 = (c - a * x_contact1) / b + y_contact2 = (c - a * x_contact2) / b + + else: + # If b == 0, we have a vertical line (ax = c), so x is constant + x_contact1 = x_contact2 = c / a + # Solve for y coordinates using the circle equation + y_contact1 = y + math.sqrt(r**2 - (x_contact1 - x) ** 2) + y_contact2 = y - math.sqrt(r**2 - (x_contact1 - x) ** 2) + + return [(x_contact1, y_contact1), (x_contact2, y_contact2)] + + +def calc_tangents_and_points(x1, y1, r1, x2, y2, r2): + dx, dy = x2 - x1, y2 - y1 # Calculate the distance components between circle centers + d_sq = dx**2 + dy**2 # Square of the distance between centers + d = math.sqrt(d_sq) # Distance between centers + + # If centers coincide or one circle is nested within the other, no common tangents exist + if d == 0 or d < abs(r1 - r2): + return [] + + tangents_and_points = [] # List to store tangents and their contact points + + # Calculate external tangents + try: + a = math.atan2(dy, dx) # Angle between the centers + b = safe_acos((r1 - r2) / d) # Adjust angle based on radii difference for external tangents + t1 = a + b + t2 = a - b + + # Calculate tangents and contact points for each angle + for t in [t1, t2]: + cos_t = math.cos(t) + sin_t = math.sin(t) + + a = cos_t # x-coefficient of tangent line + b = sin_t # y-coefficient of tangent line + c = a * x1 + b * y1 + r1 # Line constant term + + # Find contact points on each circle + points1 = calc_tangent_contact_points(x1, y1, r1, a, b, c) + points2 = calc_tangent_contact_points(x2, y2, r2, a, b, c) + if points1 is not None and points2 is not None: + tangents_and_points.append( + { + "tangent": (a, b, c), + "contact_point1": points1[0], + "contact_point2": points2[0], + } + ) + except ValueError: + pass # Handle cases where no solution exists for external tangents + + # Calculate internal tangents + try: + c = safe_acos((r1 + r2) / d) # Adjust angle based on radii sum for internal tangents + t3 = a + c + t4 = a - c + + # Calculate tangents and contact points for each angle + for t in [t3, t4]: + cos_t = math.cos(t) + sin_t = math.sin(t) + + a = cos_t # x-coefficient of tangent line + b = sin_t # y-coefficient of tangent line + c = a * x1 + b * y1 + r1 # Line constant term + + # Find contact points on each circle + points1 = calc_tangent_contact_points(x1, y1, r1, a, b, c) + points2 = calc_tangent_contact_points(x2, y2, r2, a, b, c) + + if points1 is not None and points2 is not None: + tangents_and_points.append( + { + "tangent": (a, b, c), + "contact_point1": points1[0], + "contact_point2": points2[0], + } + ) + except ValueError: + pass # Handle cases where no solution exists for internal tangents + + return tangents_and_points # Return list of tangents and contact points + + +def calc_circum_circle_tangent_to_two_circles(x1, y1, x2, y2, r, r_ext): + # Distance between the centers of the two circles + d = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + + # Check for concentric circles, where an circum_circle is not possible + if d == 0: + raise ValueError("An circum_circle does not exist for concentric circles.") + + # Calculate the center of the circum_circle + R_r = r_ext - r # Difference between the circum_circle radius and the given circles' radius + theta = np.arctan2( + np.sqrt(R_r**2 - (d / 2) ** 2), d / 2 + ) # Angle relative to the line between centers + + # Coordinates of the circum_circle’s center + x_center = (x2 - R_r * np.cos(theta) + x1 - R_r * np.cos(np.pi - theta)) / 2 + y_center = (y2 - R_r * np.sin(theta) + y1 - R_r * np.sin(np.pi - theta)) / 2 + + # Calculate the contact point coordinates on each circle + contact_1_x = x1 + r * np.cos(np.pi - theta) + contact_1_y = y1 + r * np.sin(np.pi - theta) + + contact_2_x = x2 + r * np.cos(theta) + contact_2_y = y2 + r * np.sin(theta) + + # Return angle of the tangent line, circum_circle center, and contact points on each circle + return ( + np.pi - 2 * theta, # Angle of the tangent line relative to the x-axis + [x_center, y_center], # Center of the circum_circle + [contact_1_x, contact_1_y], # Contact point on the first circle + [contact_2_x, contact_2_y], # Contact point on the second circle + ) + + +def calc_two_circles_one_line_trajectory( + center_point1, + r1, + theta_1_start, + theta_1_end, + center_point2, + r2, + theta_2_start, + theta_2_end, + step, + amplitude=0.001, +): + # Generate a trajectory connecting two circles and a straight line segment. + + # Calculate the endpoint of the first circle's arc + B = [center_point1[0] + r1 * np.cos(theta_1_end), center_point1[1] + r1 * np.sin(theta_1_end)] + + # Calculate the starting point of the second circle's arc + C = [ + center_point2[0] + r2 * np.cos(theta_2_start), + center_point2[1] + r2 * np.sin(theta_2_start), + ] + + # Compute lengths of individual segments + circle_AB = r1 * abs(theta_1_end - theta_1_start) # Arc length of the first circle + circle_CD = r2 * abs(theta_2_end - theta_2_start) # Arc length of the second circle + BC = np.linalg.norm(np.array(B) - np.array(C)) # Length of the straight-line segment + + # Calculate total trajectory distance + total_distance = circle_AB + BC + circle_CD + + # Create a time array to parameterize the trajectory + t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") + + # Initialize arrays to store trajectory points and attributes + x = np.zeros(len(t_array)) + y = np.zeros(len(t_array)) + yaw = np.zeros(len(t_array)) + curvature = np.zeros(len(t_array)) + achievement_rates = np.zeros(len(t_array)) + parts = np.zeros(len(t_array), dtype=object) + i_end = len(t_array) + + # Generate trajectory points + for i, t in enumerate(t_array): + if t > total_distance: # Stop if total distance is exceeded + i_end = i + break + + if t <= circle_AB: # Trajectory on the first circle + t1 = t + alpha = t1 / circle_AB + t1_rad = (1 - alpha) * theta_1_start + alpha * theta_1_end # Interpolate angle + x[i] = center_point1[0] + r1 * np.cos(t1_rad) + y[i] = center_point1[1] + r1 * np.sin(t1_rad) + achievement_rates[i] = t / total_distance + parts[i] = "circle" # Label as circle segment + + elif circle_AB < t <= circle_AB + BC: # Trajectory on the straight line + t2 = t - circle_AB + alpha = t2 / BC + x[i] = (1 - alpha) * B[0] + alpha * C[0] # Linear interpolation for x + y[i] = (1 - alpha) * B[1] + alpha * C[1] # Linear interpolation for y + achievement_rates[i] = t / total_distance + parts[i] = "linear" # Label as linear segment + + elif circle_AB + BC < t <= total_distance: # Trajectory on the second circle + t3 = t - (circle_AB + BC) + alpha = t3 / circle_CD + t3_rad = (1 - alpha) * theta_2_start + alpha * theta_2_end # Interpolate angle + x[i] = center_point2[0] + r2 * np.cos(t3_rad) + y[i] = center_point2[1] + r2 * np.sin(t3_rad) + achievement_rates[i] = t / total_distance + parts[i] = "circle" # Label as circle segment + + # Trim arrays to the valid trajectory length + x = x[:i_end] + y = y[:i_end] + parts = parts[:i_end] + achievement_rates = achievement_rates[:i_end] / achievement_rates[-1] # Normalize progress + + # Add sinusoidal perturbations to the trajectory + delta_x = amplitude * np.sin(4.0 * np.pi * achievement_rates) * np.sin(yaw) + delta_y = amplitude * np.sin(4.0 * np.pi * achievement_rates) * np.cos(yaw) + x += delta_x + y += delta_y + + # Calculate derivatives for yaw and curvature + dx = (x[1:] - x[:-1]) / step + dy = (y[1:] - y[:-1]) / step + ddx = (dx[1:] - dx[:-1]) / step + ddy = (dy[1:] - dy[:-1]) / step + + yaw = np.arctan2(dy, dx) + yaw = np.array(yaw.tolist() + [yaw[-1]]) # Append last value for consistent length + + curvature = ( + 1e-9 + abs(ddx * dy[:-1] - ddy * dx[:-1]) / (dx[:-1] ** 2 + dy[:-1] ** 2 + 1e-9) ** 1.5 + ) + curvature = np.array(curvature.tolist() + [curvature[-2], curvature[-1]]) # Append values + + yaw = yaw[:i_end] + curvature = curvature[:i_end] + + # Return trajectory details + return np.array([x, y]).T, yaw, curvature, parts, achievement_rates, total_distance + + +def calc_two_circles_circum_circle_trajectory( + center_point1, + r1, + theta_1_start, + theta_1_end, + center_point2, + r2, + theta_2_start, + theta_2_end, + circum_circle_center_point, + ext_r, + theta_circum_circle_start, + theta_circum_circle_end, + step, + amplitude=0.001, +): + # Generate a trajectory connecting two circular arcs and an circum_circle arc. + + # Calculate arc lengths of the first circle, circum_circle, and second circle + circle_AB = r1 * abs(theta_1_end - theta_1_start) # Arc length of the first circle + circle_BC = ext_r * abs( + theta_circum_circle_end - theta_circum_circle_start + ) # Arc length of the circum_circle + circle_CD = r2 * abs(theta_2_end - theta_2_start) # Arc length of the second circle + + # Calculate the total distance of the trajectory + total_distance = circle_AB + circle_BC + circle_CD + + # Create a time array to parametrize the trajectory + t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") + + # Initialize arrays to store trajectory details + x = np.zeros(len(t_array)) # x-coordinates + y = np.zeros(len(t_array)) # y-coordinates + yaw = np.zeros(len(t_array)) # yaw angles + curvature = np.zeros(len(t_array)) # curvature values + achievement_rates = np.zeros(len(t_array)) # progress rates + parts = np.zeros(len(t_array), dtype=object) # trajectory part labels + + i_end = len(t_array) # Valid index range for trajectory points + + # Generate trajectory points + for i, t in enumerate(t_array): + if t > total_distance: # Stop if total distance is exceeded + i_end = i + break + + if t <= circle_AB: # On the first circle arc + t1 = t + alpha = t1 / circle_AB + t1_rad = (1 - alpha) * theta_1_start + alpha * theta_1_end # Interpolate angle + x[i] = center_point1[0] + r1 * np.cos(t1_rad) + y[i] = center_point1[1] + r1 * np.sin(t1_rad) + achievement_rates[i] = t / total_distance + parts[i] = "circle" # Label as circle part + + elif circle_AB < t <= circle_AB + circle_BC: # On the circum_circle arc + kappa = 0.0 / ext_r # Optional curvature scaling factor (not used here) + t2 = t - circle_AB + alpha = t2 / circle_BC + t2_rad = ( + 1 - alpha + ) * theta_circum_circle_start + alpha * theta_circum_circle_end # Interpolate angle + x[i] = circum_circle_center_point[0] + ( + 1.0 + kappa * 4 * alpha * (1 - alpha) * 4 * alpha * (1 - alpha) + ) * ext_r * np.cos(t2_rad) + y[i] = circum_circle_center_point[1] + ( + 1.0 + kappa * 4 * alpha * (1 - alpha) * 4 * alpha * (1 - alpha) + ) * ext_r * np.sin(t2_rad) + achievement_rates[i] = t / total_distance + parts[i] = "linear" # Label as linear part (for simplicity) + + elif circle_AB + circle_BC < t <= total_distance: # On the second circle arc + t3 = t - (circle_AB + circle_BC) + alpha = t3 / circle_CD + t3_rad = (1 - alpha) * theta_2_start + alpha * theta_2_end # Interpolate angle + x[i] = center_point2[0] + r2 * np.cos(t3_rad) + y[i] = center_point2[1] + r2 * np.sin(t3_rad) + achievement_rates[i] = t / total_distance + parts[i] = "circle" # Label as circle part + + # Trim arrays to the valid trajectory range + x = x[:i_end] + y = y[:i_end] + parts = parts[:i_end] + achievement_rates = achievement_rates[:i_end] + + # Add sinusoidal perturbations to the trajectory + delta_x = ( + amplitude * np.sin(4.0 * np.pi * achievement_rates) + + 0.05 * amplitude * np.sin(16.0 * np.pi * achievement_rates) + ) * np.sin(yaw) + delta_y = ( + amplitude * np.sin(4.0 * np.pi * achievement_rates) + + 0.05 * amplitude * np.sin(16.0 * np.pi * achievement_rates) + ) * np.cos(yaw) + x += delta_x + y += delta_y + + # Compute derivatives for yaw and curvature + dx = (x[1:] - x[:-1]) / step + dy = (y[1:] - y[:-1]) / step + ddx = (dx[1:] - dx[:-1]) / step + ddy = (dy[1:] - dy[:-1]) / step + + yaw = np.arctan2(dy, dx) + yaw = np.array(yaw.tolist() + [yaw[-1]]) # Append the last value for consistency + + curvature = ( + 1e-9 + abs(ddx * dy[:-1] - ddy * dx[:-1]) / (dx[:-1] ** 2 + dy[:-1] ** 2 + 1e-9) ** 1.5 + ) + curvature = np.array(curvature.tolist() + [curvature[-2], curvature[-1]]) # Append values + yaw = yaw[:i_end] + curvature = curvature[:i_end] + + # Return trajectory details + return np.array([x, y]).T, yaw, curvature, parts, achievement_rates, total_distance + + +class TrajectorySegment: + def __init__( + self, + segment_type: str, + trajectory, + yaw, + curvature, + parts, + achievement_rates, + in_direction: str, + out_direction: str, + reversing: bool, + distance: float, + ): + # Validation for segment_type + if segment_type not in ["boundary", "non_boundary"]: + raise ValueError("segment_type must be 'boundary' or 'non_boundary'") + + # Validation for in_direction and out_direction + if in_direction not in ["clock_wise", "counter_clock_wise"]: + raise ValueError("in_direction must be 'clock_wise' or 'counter_clock_wise'") + if out_direction not in ["clock_wise", "counter_clock_wise"]: + raise ValueError("out_direction must be 'clock_wise' or 'counter_clock_wise'") + + self.type = segment_type + self.trajectory = trajectory + self.yaw = yaw + self.curvature = curvature + self.parts = parts + self.achievement_rates = achievement_rates + self.in_direction = in_direction + self.out_direction = out_direction + self.reversing = reversing + self.distance = distance + + def __repr__(self): + return ( + f"TrajectorySegment(type={self.type}, trajectory={self.trajectory}, yaw={self.yaw}, " + f"curvature={self.curvature}, parts={self.parts}, achievement_rates={self.achievement_rates}, " + f"in_direction={self.in_direction}, out_direction={self.out_direction}, reversing={self.reversing}), distance={self.distance}" + ) + + +def calc_two_circles_common_tangent_trajectory(R, r, step, amplitude=0.001): + # Case 1: Smaller circle is inside the larger circle (r < R) + if r < R: + # Define centers of the two circles + center_1 = [-(R - r), 0.0] # Center of the first circle + center_2 = [(R - r), 0.0] # Center of the second circle + + # Calculate tangents and contact points between the two circles + tangents_and_points = calc_tangents_and_points( + center_1[0], center_1[1], r, center_2[0], center_2[1], r + ) + + # Iterate through the possible tangents + for tangent_and_point in tangents_and_points: + contact_point1 = tangent_and_point["contact_point1"] + contact_point2 = tangent_and_point["contact_point2"] + + # Condition 1: Specific tangent selection based on y-coordinates and radius + if contact_point1[1] > 0.0 and contact_point2[1] < 0.0 and r < R / 2: + # Compute angles for the start and end of the trajectory + theta_1_end = np.arctan2( + contact_point1[1] - center_1[1], contact_point1[0] - center_1[0] + ) + theta_2_start = np.arctan2( + contact_point2[1] - center_2[1], contact_point2[0] - center_2[0] + ) + + # Calculate the trajectory for this configuration + ( + trajectory, + yaw, + curvature, + parts, + achievement_rates, + total_distance, + ) = calc_two_circles_one_line_trajectory( + center_1, + r, + np.pi, + theta_1_end, + center_2, + r, + theta_2_start, + 0.0, + step=step, + amplitude=amplitude, + ) + + # Return a TrajectorySegment object with all calculated details + return TrajectorySegment( + "non_boundary", + trajectory, + yaw, + curvature, + parts, + achievement_rates, + "clock_wise", + "counter_clock_wise", + True, + total_distance, + ) + + # Condition 2: Alternative tangent selection for a different configuration + if contact_point1[1] > 0.0 and contact_point2[1] > 0.0 and R / 2 <= r < R: + # Compute angles for the start and end of the trajectory + theta_1_end = np.arctan2( + contact_point1[1] - center_1[1], contact_point1[0] - center_1[0] + ) + theta_2_start = np.arctan2( + contact_point2[1] - center_2[1], contact_point2[0] - center_2[0] + ) + + # Calculate the trajectory for this configuration + ( + trajectory, + yaw, + curvature, + parts, + achievement_rates, + total_distance, + ) = calc_two_circles_one_line_trajectory( + center_1, + r, + np.pi, + theta_1_end, + center_2, + r, + theta_2_start, + 0.0, + step=step, + amplitude=amplitude, + ) + + # Return a TrajectorySegment object with all calculated details + return TrajectorySegment( + "non_boundary", + trajectory, + yaw, + curvature, + parts, + achievement_rates, + "clock_wise", + "clock_wise", + False, + total_distance, + ) + + # Case 2: Smaller circle is outside or overlaps the larger circle (R <= r) + elif R <= r: + # Define centers for the two circles + center_1 = [-(R - R * 0.75), 0.0] # Adjusted center of the first circle + center_2 = [(R - R * 0.75), 0.0] # Adjusted center of the second circle + + # Calculate external tangent and points for the circum_circle + ( + theta, + (x_center, y_center), + (contact_1_x, contact_1_y), + (contact_2_x, contact_2_y), + ) = calc_circum_circle_tangent_to_two_circles( + center_1[0], center_1[1], center_2[0], center_2[1], R * 0.75, r + ) + + # Compute angles for the start and end of the trajectory + theta_1_end = np.arctan2(contact_1_y - center_1[1], contact_1_x - center_1[0]) + theta_2_start = np.arctan2(contact_2_y - center_2[1], contact_2_x - center_2[0]) + + # Calculate the trajectory for the circum_circle configuration + ( + trajectory, + yaw, + curvature, + parts, + achievement_rates, + total_distance, + ) = calc_two_circles_circum_circle_trajectory( + center_point1=center_1, + r1=R * 0.75, + theta_1_start=np.pi, + theta_1_end=theta_1_end, + center_point2=center_2, + r2=R * 0.75, + theta_2_start=theta_2_start, + theta_2_end=0.0, + circum_circle_center_point=[x_center, y_center], + ext_r=r, + theta_circum_circle_start=np.pi - (np.pi - theta) / 2, + theta_circum_circle_end=(np.pi - theta) / 2, + step=step, + amplitude=amplitude, + ) + + # Return a TrajectorySegment object with all calculated details + return TrajectorySegment( + "non_boundary", + trajectory, + yaw, + curvature, + parts, + achievement_rates, + "clock_wise", + "clock_wise", + False, + total_distance, + ) + + # Return None if no valid trajectory can be calculated + return None + + +def calc_boundary_trajectory(R): + # Define the angular change for the boundary trajectory + delta_theta = 1.0 # Angle span in radians + circle = R * delta_theta # Arc length of the circular segment + + # Total distance of the trajectory + total_distance = circle + + # Generate a time array for parameterization + t_array = np.arange(start=0.0, stop=total_distance, step=0.10).astype("float") + + # Initialize trajectory arrays + x = np.zeros(len(t_array)) # X-coordinates of the trajectory + y = np.zeros(len(t_array)) # Y-coordinates of the trajectory + yaw = np.zeros(len(t_array)) # Yaw (orientation) at each point + curvature = np.zeros(len(t_array)) # Curvature at each point + achievement_rates = np.zeros(len(t_array)) # Progress along the trajectory + parts = np.zeros(len(t_array), dtype=object) # Labels for trajectory parts + + i_end = len(t_array) # Index where the trajectory ends + + # Define the start and end angles for the circular arc + theta_start = np.pi # Starting angle + theta_end = np.pi - delta_theta # Ending angle + + # Generate the trajectory + for i, t in enumerate(t_array): + if t > total_distance: # Stop if the distance exceeds the total arc length + i_end = i + break + + if t <= circle: # Arc trajectory generation + t1 = t # Distance traveled along the arc + alpha = t1 / circle # Normalized progress along the arc + t1_rad = (1 - alpha) * theta_start + alpha * theta_end # Interpolated angle + + # Calculate X, Y coordinates + x[i] = R * np.cos(t1_rad) + y[i] = R * np.sin(t1_rad) + + # Calculate tangent vectors for yaw computation + tangent_x = ( + np.cos(np.pi / 2 + t1_rad) + * (theta_end - theta_start) + / abs(theta_end - theta_start) + ) + tangent_y = ( + np.sin(np.pi / 2 + t1_rad) + * (theta_end - theta_start) + / abs(theta_end - theta_start) + ) + yaw[i] = math.atan2(tangent_y, tangent_x) # Calculate yaw from tangent + + # Set curvature (constant for circular arc) + curvature[i] = 1 / R + + # Progress along the trajectory as a fraction of total distance + achievement_rates[i] = t / total_distance + + # Label this segment as part of the "circle" + parts[i] = "circle" + + # Trim arrays to the valid trajectory length + x = x[:i_end] + y = y[:i_end] + yaw = yaw[:i_end] + curvature = curvature[:i_end] + parts = parts[:i_end] + achievement_rates = achievement_rates[:i_end] + + # Return the trajectory segment object + return TrajectorySegment( + "boundary", # Segment type + np.array([x, y]).T, # Trajectory points + yaw, # Yaw angles + curvature, # Curvatures + parts, # Segment labels + achievement_rates, # Achievement rates + "clock_wise", # Direction of rotation for this arc + "clock_wise", # Direction of rotation for a hypothetical next arc + False, # Flag for boundary inclusion + total_distance, # Total distance of the trajectory + ) + + +def reverse_trajectory_segment(trajectory_segment): + # Define the rotation matrix to reverse the trajectory + # This flips the trajectory along the X-axis + Rot = np.array([[1.0, 0.0], [0.0, -1.0]]) + + # Reverse direction initialization + # Start by assuming the reversed directions are "clock_wise" + in_direction_reversed = "clock_wise" + out_direction_reversed = "clock_wise" + + # Reverse the "in_direction" + if trajectory_segment.in_direction == "clock_wise": + in_direction_reversed = "counter_clock_wise" + elif trajectory_segment.in_direction == "counter_clock_wise": + in_direction_reversed = "clock_wise" + + # Reverse the "out_direction" + if trajectory_segment.out_direction == "clock_wise": + out_direction_reversed = "counter_clock_wise" + elif trajectory_segment.out_direction == "counter_clock_wise": + out_direction_reversed = "clock_wise" + + # Create a deep copy of the input trajectory segment + # This ensures the original trajectory_segment remains unmodified + trajectory_segment_reversed = copy.deepcopy(trajectory_segment) + + # Apply the rotation to reverse the trajectory + # Multiply the trajectory's coordinates by the rotation matrix + trajectory_segment_reversed.trajectory = trajectory_segment.trajectory @ Rot + + # Reverse the yaw (orientation) for all points in the trajectory + trajectory_segment_reversed.yaw = -trajectory_segment.yaw + + # Update the reversed directions + trajectory_segment_reversed.in_direction = in_direction_reversed + trajectory_segment_reversed.out_direction = out_direction_reversed + + # Return the reversed trajectory segment + return trajectory_segment_reversed + + +def declare_reversal_loop_circle_params(node): + node.declare_parameter( + "trajectory_radius", + 35.0, + ParameterDescriptor( + description="Radius of the circle where trajectories are generated [m]" + ), + ) + + node.declare_parameter( + "enclosing_radius", + 40.0, + ParameterDescriptor( + description="Radius of the circle enclosing the generated trajectories [m]" + ), + ) + + node.declare_parameter( + "look_ahead_distance", + 15.0, + ParameterDescriptor( + description="The distance referenced ahead of the vehicle for collecting steering angle data" + ), + ) + + +class Reversal_Loop_Circle(Base_Course): + def __init__(self, step: float, param_dict): + # Initialize the Reversal_Loop_Circle class and set up trajectories for straight, turning, and boundary paths. + + # Call the constructor of the parent class (Base_Course). + super().__init__(step, param_dict) + + # Initialize general parameters. + self.closed = False # Indicates whether the trajectory is closed. + self.wheel_base = param_dict["wheel_base"] # Vehicle's wheel base. + self.trajectory_R = param_dict[ + "trajectory_radius" + ] # Circle radius for trajectory generation. + self.enclosing_R = param_dict["enclosing_radius"] + self.vel_hist = deque([float(0.0)] * 30, maxlen=30) # Velocity history for smoothing. + self.previous_updated_time = 0.0 # Timestamp for tracking updates. + + # Initialize velocity and acceleration targets for trajectory segmentation. + self.target_vel_on_segmentation = 6.0 # Target velocity. + self.target_acc_on_segmentation = 0.0 # Target acceleration. + self.vel_idx, self.acc_idx = 0, 0 # Indices for velocity and acceleration bins. + + # Set the initial vehicle phase and other state variables. + self.vehicle_phase = "acceleration" # Vehicle's current motion phase. + self.const_velocity_start_time = 0.0 # Start time for constant velocity phase. + self.updated_target_velocity = ( + False # Indicates whether the target velocity has been updated. + ) + + self.look_ahead_distance = param_dict[ + "look_ahead_distance" + ] # Distance to look ahead for collecting steer angle. + + # Generate nearly straight trajectories inside the circle. + + self.steer_list = [ + 0.001, + 0.01, + 0.02, + 0.03, + ] # List of steering angles for nearly straight paths. + self.trajectory_nearly_straight_clock_wise = ( + {} + ) # Clockwise trajectories for nearly straight paths. + self.trajectory_nearly_straight_counter_clock_wise = ( + {} + ) # Counterclockwise trajectories for nearly straight paths. + + for i in range(len(self.steer_list)): + # Calculate the radius of curvature for a given steering angle. + r = self.wheel_base / np.tan(self.steer_list[i]) + + # Generate and store clockwise trajectories. + trajectory = calc_two_circles_common_tangent_trajectory( + self.trajectory_R, r, step=self.step + ) + self.trajectory_nearly_straight_clock_wise[self.steer_list[i]] = trajectory + + # Generate and store counterclockwise trajectories by reversing the clockwise trajectory. + self.trajectory_nearly_straight_counter_clock_wise[ + self.steer_list[i] + ] = reverse_trajectory_segment(trajectory) + + # Generate trajectories for changing directions (turning). + + self.trajectory_for_turning = {} # Store trajectories for direction changes. + self.amplitude_list = [] # List of amplitude values for turning. + self.turning_steer_list = [] # List of steering angles for turning. + + for i in range(200): + amplitude = 0.1 * i # Gradually increase the amplitude for trajectory variations. + self.amplitude_list.append(str(amplitude)) + + # Create a trajectory dictionary for transitions between directions. + trajectory_for_turning__ = {} + r = self.trajectory_R / 2 * 0.75 # Use a reduced radius for sharper turns. + trajectory = calc_two_circles_common_tangent_trajectory( + self.trajectory_R, r, step=self.step, amplitude=amplitude + ) + + # Generate clockwise-to-counterclockwise turning trajectories. + trajectory_for_turning__["clock_wise_to_counter_clock_wise"] = trajectory + + # Generate counterclockwise-to-clockwise turning trajectories by reversing. + trajectory_reversed = reverse_trajectory_segment(trajectory) + trajectory_for_turning__["counter_clock_wise_to_clock_wise"] = trajectory_reversed + + # Store the trajectories for turning. + self.trajectory_for_turning[str(amplitude)] = trajectory_for_turning__ + + # Calculate and store the median turning steering angle. + self.turning_steer_list.append( + np.arctan2( + ( + 0.5 * statistics.median(trajectory_reversed.curvature) + + 0.5 * np.max(trajectory_reversed.curvature) + ) + * self.wheel_base, + 1.0, + ) + ) + + # Generate boundary trajectories. + + self.boundary_trajectory = {} # Store boundary trajectories. + + # Generate clockwise boundary trajectory. + trajectory = calc_boundary_trajectory(self.trajectory_R) + self.boundary_trajectory["clock_wise"] = trajectory + + # Generate counterclockwise boundary trajectory by reversing. + trajectory_reversed = reverse_trajectory_segment(trajectory) + self.boundary_trajectory["counter_clock_wise"] = trajectory_reversed + + # Initialize the trajectory list with a boundary trajectory. + + self.trajectory_list = [ + self.boundary_trajectory["clock_wise"] + ] # Start with a clockwise boundary trajectory. + self.end_point = self.trajectory_list[-1].trajectory[ + -1 + ] # Endpoint of the initial trajectory. + self.latest_direction = "clock_wise" # Initial direction. + self.trajectory_length_list = [ + len(self.trajectory_list[-1].trajectory) + ] # Track trajectory lengths. + + # Add initial straight and turning trajectories to the trajectory list. + while len(self.trajectory_length_list) < 3: + self.add_trajectory_nearly_straight() # Add nearly straight trajectories. + self.add_trajectory_nearly_straight() + steer_of_trajectory = 1e-9 # Minimal steer value for turning initialization. + self.add_trajectory_for_turning(steer_of_trajectory) + + def add_trajectory(self, trajectory): + # Add a new trajectory to the trajectory list, ensuring proper alignment and continuity. + + # If the last trajectory and the new trajectory are both non-boundary types, + # add a boundary trajectory to ensure proper alignment. + if self.trajectory_list[-1].type == "non_boundary" and trajectory.type == "non_boundary": + self.add_trajectory(self.boundary_trajectory[self.latest_direction]) + + # If the incoming trajectory's direction is different from the latest trajectory's direction, + # and the latest trajectory is a boundary type, add a turning trajectory to connect them. + if ( + trajectory.in_direction is not self.latest_direction + and self.trajectory_list[-1].type == "boundary" + ): + key_ = self.latest_direction + "_to_" + getattr(trajectory, "in_direction") + self.add_trajectory(self.trajectory_for_turning[self.amplitude_list[0]][key_]) + + # Ensure proper alignment if there are consecutive non-boundary trajectories. + if self.trajectory_list[-1].type == "non_boundary" and trajectory.type == "non_boundary": + self.add_trajectory(self.boundary_trajectory[self.latest_direction]) + + # Compute rotation to align the new trajectory with the end of the last trajectory. + cos = self.end_point[0] / self.trajectory_R # Cosine of the angle for alignment + sin = self.end_point[1] / self.trajectory_R # Sine of the angle for alignment + delta_theta = np.arctan2(sin, cos) - np.pi # Rotation angle + Rotation = (-np.eye(2)) @ np.array([[cos, -sin], [sin, cos]]) # Rotation matrix + + # Create a modified copy of the input trajectory for alignment. + trajectory_modified = copy.deepcopy(trajectory) + trajectory_modified.trajectory = (Rotation @ trajectory.trajectory.T).T + trajectory_modified.yaw = trajectory.yaw + delta_theta + + # Update the latest direction and endpoint for the trajectory. + self.latest_direction = trajectory.out_direction + self.end_point = trajectory_modified.trajectory[-1] + + # Add the modified trajectory to the list and update the length list. + self.trajectory_list.append(trajectory_modified) + self.trajectory_length_list.append(len(trajectory_modified.trajectory)) + + def add_trajectory_nearly_straight(self): + # Add a nearly straight trajectory to the trajectory list, depending on the current direction. + + if self.latest_direction == "clock_wise": + # Add a trajectory for a clockwise direction using a random steering angle. + self.add_trajectory( + self.trajectory_nearly_straight_clock_wise[random.choice(self.steer_list)] + ) + elif self.latest_direction == "counter_clock_wise": + # Add a trajectory for a counterclockwise direction using a random steering angle. + self.add_trajectory( + self.trajectory_nearly_straight_counter_clock_wise[random.choice(self.steer_list)] + ) + + def remove_trajectory(self): + # Remove the oldest trajectory from the trajectory list to manage memory and data size. + + self.trajectory_list.pop(0) + self.trajectory_length_list.pop(0) + + def add_trajectory_for_turning(self, steer): + # Add a trajectory for transitioning between clockwise and counterclockwise directions. + + # Find the index of the closest available steering angle in the turning list. + index = (np.abs(np.array(self.turning_steer_list) - steer)).argmin() + + if self.latest_direction == "clock_wise": + # Add a trajectory for transitioning from clockwise to counterclockwise direction. + self.add_trajectory( + self.trajectory_for_turning[self.amplitude_list[index]][ + self.latest_direction + "_to_" + "counter_clock_wise" + ] + ) + elif self.latest_direction == "counter_clock_wise": + # Add a trajectory for transitioning from counterclockwise to clockwise direction. + self.add_trajectory( + self.trajectory_for_turning[self.amplitude_list[index]][ + self.latest_direction + "_to_" + "clock_wise" + ] + ) + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point=np.array([0.0, 0.0]), + goal_point=np.array([0.0, 0.0]), + ): + # Concatenate the trajectory points from the first four trajectory segments + self.trajectory_points = np.concatenate( + [self.trajectory_list[i].trajectory for i in range(4)], + axis=0, + ) + # Concatenate yaw angles from the first four trajectory segments + self.yaw = np.concatenate( + [self.trajectory_list[i].yaw for i in range(4)], + axis=0, + ) + # Concatenate parts (labels) from the first four trajectory segments + self.parts = np.concatenate([self.trajectory_list[i].parts for i in range(4)], axis=0) + # Concatenate curvatures from the first four trajectory segments + self.curvature = np.concatenate( + [self.trajectory_list[i].curvature for i in range(4)], axis=0 + ) + # Concatenate achievement rates from the first four trajectory segments + self.achievement_rates = np.concatenate( + [self.trajectory_list[i].achievement_rates for i in range(4)], + axis=0, + ) + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + # Calculate the target velocity for the vehicle based on its current state, trajectory phase, and constraints such as acceleration limits and trajectory curvature. + + # Initialize target acceleration and velocity if not already updated + if not self.updated_target_velocity: + # Choose velocity and acceleration bins based on collected data + self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( + collected_data_counts_of_vel_acc + ) + self.target_acc_on_segmentation = self.params.a_bin_centers[self.acc_idx] + self.target_vel_on_segmentation = self.params.v_bin_centers[self.vel_idx] + self.acc_on_line = self.target_acc_on_segmentation + + # Set the vehicle's phase to "acceleration" + self.vehicle_phase = "acceleration" + self.updated_target_velocity = True + + acc_kp_of_pure_pursuit = self.params.acc_kp # Proportional gain for acceleration control + T = 10.0 # Period of the sine wave used to modulate velocity + sine = np.sin(2 * np.pi * current_time / T) # Sine wave for smooth velocity modulation + + # Handle acceleration phase + if self.vehicle_phase == "acceleration": + if current_vel < self.target_vel_on_segmentation - 1.0 * abs( + self.target_acc_on_segmentation + ): + # Increase velocity with a maximum allowable acceleration + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.50 * sine + ) + else: + # Increase velocity with a absolute target acceleration + target_vel = current_vel + abs( + self.target_acc_on_segmentation + ) / acc_kp_of_pure_pursuit * (1.25 + 0.50 * sine) + + # Transition to "constant speed" phase once the target velocity is reached + if current_vel > self.target_vel_on_segmentation: + self.vehicle_phase = "constant_speed" + self.const_velocity_start_time = current_time + + # Handle constant speed phase + if self.vehicle_phase == "constant_speed": + # Modulate velocity around the target with a sine wave + target_vel = self.target_vel_on_segmentation + 2.0 * (-0.5 + 1.0 * sine) + + # Transition to "deceleration" phase after a fixed duration + if current_time - self.const_velocity_start_time > T: + self.vehicle_phase = "deceleration" + + # Handle deceleration phase + if self.vehicle_phase == "deceleration": + if current_vel < self.target_vel_on_segmentation - 1.0 * abs( + self.target_acc_on_segmentation + ): + # Decrease velocity with a maximum deceleration + target_vel = current_vel - self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.50 * sine + ) + else: + # Decrease velocity with a absolute target acceleration + target_vel = current_vel - abs( + self.target_acc_on_segmentation + ) / acc_kp_of_pure_pursuit * (1.25 + 0.50 * sine) + + # Reset velocity update flag when deceleration is complete + if ( + current_vel + < self.target_vel_on_segmentation + - 1.0 * abs(self.target_acc_on_segmentation) / acc_kp_of_pure_pursuit + ): + self.updated_target_velocity = False + + # Maintain a smoothed velocity by averaging recent values + self.vel_hist.append(target_vel) + target_vel = np.mean(self.vel_hist) + + # Special handling for trajectory direction changes + if self.trajectory_list[2].in_direction is not self.trajectory_list[2].out_direction: + # Set a fixed target velocity during direction transitions + target_vel = 3.0 + 1.0 * sine + + # Adjust velocity based on trajectory curvature and lateral acceleration constraints + if (self.trajectory_list[1].in_direction is not self.trajectory_list[1].out_direction) or ( + self.trajectory_list[2].in_direction is not self.trajectory_list[2].out_direction + ): + max_curvature_on_segment = 1e-9 # Initialize to a small value to avoid division by zero + max_lateral_vel_on_segment = 1e9 # Initialize to a large value as a placeholder + + # Compute the maximum curvature and lateral velocity in a look-ahead segment + if self.yaw is not None and self.curvature is not None: + steer_sign = np.sign( + self.yaw[nearestIndex + 1] - self.yaw[nearestIndex] + ) # Steering direction + max_curvature_on_segment = np.max( + self.curvature[ + nearestIndex : nearestIndex + int(self.look_ahead_distance / self.step) + ] + ) + max_lateral_vel_on_segment = np.sqrt( + self.params.max_lateral_accel / max_curvature_on_segment + ) + + # Identify the velocity index for the maximum lateral velocity + max_vel_idx = np.clip( + np.digitize(max_lateral_vel_on_segment, self.params.v_bins) - 1, + 0, + self.params.collecting_data_max_n_v - 1, + ) + + # Compute steering angle and its corresponding index + steer = np.arctan(self.wheel_base * max_curvature_on_segment) * steer_sign + steer_idx = np.clip( + np.digitize(steer, self.params.steer_bins) - 1, 0, len(self.params.steer_bins) - 2 + ) + + # Determine the minimum velocity based on collected data + min_vel = np.min(collected_data_counts_of_vel_steer[:max_vel_idx, steer_idx]) + vel_idx = np.where( + collected_data_counts_of_vel_steer[:max_vel_idx, steer_idx] == min_vel + )[0] + target_vel_ = self.params.v_bin_centers[vel_idx[0]] + + # Constrain velocity based on lateral acceleration limits + max_vel_from_lateral_acc_on_segment = np.sqrt( + self.params.max_lateral_accel / max_curvature_on_segment + ) + target_vel = np.min([target_vel_ + 0.5 * sine, max_vel_from_lateral_acc_on_segment]) + + # Ensure the target velocity remains above a minimum threshold + target_vel = np.max([target_vel, 0.5]) + + return target_vel + + def update_trajectory_points( + self, + nearestIndex, + yaw_offset, + rectangle_center_position, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + # Updates the trajectory points based on the vehicle's progress. + # Adds new trajectory segments as needed and updates the final trajectory data. + + # If the current position exceeds the length of the first two trajectory segments + if nearestIndex > self.trajectory_length_list[0] + self.trajectory_length_list[1]: + # Remove the oldest trajectory segment + nearestIndex = nearestIndex - self.trajectory_length_list[0] + self.remove_trajectory() + + # Find the steering angle with the least amount of collected data + mini_num_data = 1e9 + steer_with_minimum_num_of_data = 1e-9 + for i in range(len(collected_data_counts_of_vel_steer[0])): + # Calculate the steering angle at index i + steer_at_idx_i = abs(self.params.steer_bin_centers[i]) + 1e-9 + + # Compute the maximum velocity allowed by lateral acceleration + max_vel_from_lateral_acc = np.sqrt( + self.params.max_lateral_accel / (self.wheel_base * np.tan(steer_at_idx_i)) + ) + + # Find the index corresponding to this maximum velocity + vel_idx = np.digitize(max_vel_from_lateral_acc, self.params.v_bins) - 1 + + # Compute the average number of data points and find the minimum + num_data = np.mean(np.minimum(collected_data_counts_of_vel_steer[:vel_idx, i], 50)) + if num_data < mini_num_data: + mini_num_data = num_data + steer_with_minimum_num_of_data = steer_at_idx_i + + # Add trajectory segments until the list has 4 segments + while len(self.trajectory_length_list) < 4: + self.add_trajectory_nearly_straight() + self.add_trajectory_nearly_straight() + self.add_trajectory_for_turning(steer_with_minimum_num_of_data) + + # Concatenate trajectory data from the trajectory list + self.trajectory_points = np.concatenate( + [self.trajectory_list[i].trajectory for i in range(4)], + axis=0, + ) + self.yaw = np.concatenate( + [self.trajectory_list[i].yaw for i in range(4)], + axis=0, + ) + self.parts = np.concatenate([self.trajectory_list[i].parts for i in range(4)], axis=0) + self.curvature = np.concatenate( + [self.trajectory_list[i].curvature for i in range(4)], axis=0 + ) + self.achievement_rates = np.concatenate( + [self.trajectory_list[i].achievement_rates for i in range(4)], axis=0 + ) + + # Return the updated nearestIndex and trajectory points + return nearestIndex, *self.return_trajectory_points(yaw_offset, rectangle_center_position) + + def get_boundary_points(self): + # Computes the boundary points around the trajectory. + # This is used to create an external boundary around the trajectory. + + # Return None if trajectory points or yaw is not available + if self.trajectory_points is None or self.yaw is None: + return None + + # Define the radius for the boundary circle + outer_circle_radius = self.enclosing_R + theta_list = np.linspace(0, 2 * np.pi, 200) + boundary_points = [] + + # Compute the center of the boundary circle + center_point_x = (self.A[0] + self.B[0] + self.C[0] + self.D[0]) / 4 + center_point_y = (self.A[1] + self.B[1] + self.C[1] + self.D[1]) / 4 + + # Calculate the points along the boundary circle + for theta in theta_list: + point_x = outer_circle_radius * np.cos(theta) + center_point_x + point_y = outer_circle_radius * np.sin(theta) + center_point_y + boundary_points.append([point_x, point_y]) + + # Store the boundary points as a numpy array + self.boundary_points = np.array(boundary_points) diff --git a/control_data_collecting_tool/scripts/courses/straight_line_negative.py b/control_data_collecting_tool/scripts/courses/straight_line_negative.py new file mode 100644 index 00000000..07edb09e --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/straight_line_negative.py @@ -0,0 +1,147 @@ +#!/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. + +from courses.base_course import Base_Course +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor + + +def computeTriangleArea(A, B, C): + return 0.5 * abs(np.cross(B - A, C - A)) + + +def declare_straight_line_negative_params(node): + node.declare_parameter( + "stopping_buffer_distance", + 10.0, + ParameterDescriptor(description="The safety distance from end of the straight line [m]"), + ) + + +class Straight_Line_Negative(Base_Course): + def __init__(self, step: float, param_dict): + super().__init__(step, param_dict) + self.closed = False + + self.target_vel_on_straight_line = 6.0 + self.target_acc_on_straight_line = 0.0 + self.vel_idx, self.acc_idx = 0, 0 + + self.deceleration_rate = 1.0 + + self.sine_period_for_velocity = 7.5 + + self.stopping_buffer_distance = param_dict["stopping_buffer_distance"] + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point=np.array([0.0, 0.0]), + goal_point=np.array([0.0, 0.0]), + ): + total_distance = long_side_length + t_array = np.arange(start=0.0, stop=total_distance, step=self.step).astype("float") + + self.yaw = np.zeros(len(t_array)) + self.parts = ["linear" for _ in range(len(t_array.copy()))] + x = np.linspace(-total_distance / 2, total_distance / 2, len(t_array)) + y = np.zeros(len(t_array)) + + self.trajectory_points = np.vstack((x, y)).T + self.curvature = 1e-9 * np.ones(len(t_array)) + self.achievement_rates = np.linspace(0.0, 1.0, len(t_array)) + + return self.trajectory_points, self.yaw, self.curvature, self.parts, self.achievement_rates + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + part = self.parts[nearestIndex] + achievement_rate = self.achievement_rates[nearestIndex] + acc_kp_of_pure_pursuit = self.params.acc_kp + + # Check and update target velocity on straight line + if part == "straight" and achievement_rate < 0.05: + self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( + collected_data_counts_of_vel_acc + ) + self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] + self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] + + distance = len(self.parts) * self.step + stop_distance = self.target_vel_on_straight_line**2 / (2 * self.params.a_max) + self.deceleration_rate = ( + 1.0 - stop_distance / distance - self.stopping_buffer_distance / distance + ) + + # Calculate sine wave and apply to velocity + T = self.sine_period_for_velocity + sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + + if current_vel > self.target_vel_on_straight_line: + target_vel = self.target_vel_on_straight_line + sine - 1.0 + target_vel = max(target_vel, 0.05) + elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( + self.target_acc_on_straight_line + ): + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.5 * sine + ) + else: + target_vel = current_vel + abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + + # Adjust for deceleration based on achievement rate + if self.deceleration_rate - 0.05 <= achievement_rate < self.deceleration_rate: + target_vel = current_vel - abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + elif self.deceleration_rate <= achievement_rate: + target_vel = ( + (current_vel - self.params.a_max / acc_kp_of_pure_pursuit) + * (1.0 + 0.5 * sine) + * (1.0 - achievement_rate) + ) + + return target_vel + + def get_boundary_points(self): + self.boundary_points = np.vstack((self.A, self.B, self.C, self.D)) + + def check_in_boundary(self, current_position): + P = current_position[0:2] + + area_ABCD = computeTriangleArea(self.A, self.B, self.C) + computeTriangleArea( + self.C, self.D, self.A + ) + + area_PAB = computeTriangleArea(P, self.A, self.B) + area_PBC = computeTriangleArea(P, self.B, self.C) + area_PCD = computeTriangleArea(P, self.C, self.D) + area_PDA = computeTriangleArea(P, self.D, self.A) + + if area_PAB + area_PBC + area_PCD + area_PDA > area_ABCD * 1.001: + return False + else: + return True diff --git a/control_data_collecting_tool/scripts/courses/straight_line_positive.py b/control_data_collecting_tool/scripts/courses/straight_line_positive.py new file mode 100644 index 00000000..b90a2dec --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/straight_line_positive.py @@ -0,0 +1,147 @@ +#!/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. + +from courses.base_course import Base_Course +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor + + +def computeTriangleArea(A, B, C): + return 0.5 * abs(np.cross(B - A, C - A)) + + +def declare_straight_line_positive_params(node): + node.declare_parameter( + "stopping_buffer_distance", + 10.0, + ParameterDescriptor(description="The safety distance from end of the straight line [m]"), + ) + + +class Straight_Line_Positive(Base_Course): + def __init__(self, step: float, param_dict): + super().__init__(step, param_dict) + self.closed = False + + self.target_vel_on_straight_line = 6.0 + self.target_acc_on_straight_line = 0.0 + self.vel_idx, self.acc_idx = 0, 0 + + self.deceleration_rate = 1.0 + + self.sine_period_for_velocity = 7.5 + + self.stopping_buffer_distance = param_dict["stopping_buffer_distance"] + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point=np.array([0.0, 0.0]), + goal_point=np.array([0.0, 0.0]), + ): + total_distance = long_side_length + t_array = np.arange(start=0.0, stop=total_distance, step=self.step).astype("float") + + self.yaw = np.ones(len(t_array)) * np.pi + self.parts = ["straight" for _ in range(len(t_array.copy()))] + x = np.linspace(total_distance / 2, -total_distance / 2, len(t_array)) + y = np.zeros(len(t_array)) + + self.trajectory_points = np.vstack((x, y)).T + self.curvature = 1e-9 * np.ones(len(t_array)) + self.achievement_rates = np.linspace(0.0, 1.0, len(t_array)) + + return self.trajectory_points, self.yaw, self.curvature, self.parts, self.achievement_rates + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + part = self.parts[nearestIndex] + achievement_rate = self.achievement_rates[nearestIndex] + acc_kp_of_pure_pursuit = self.params.acc_kp + + # Check and update target velocity on straight line + if part == "straight" and achievement_rate < 0.05: + self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( + collected_data_counts_of_vel_acc + ) + self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] + self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] + + distance = len(self.parts) * self.step + stop_distance = self.target_vel_on_straight_line**2 / (2 * self.params.a_max) + self.deceleration_rate = ( + 1.0 - stop_distance / distance - self.stopping_buffer_distance / distance + ) + + # Calculate sine wave and apply to velocity + T = self.sine_period_for_velocity + sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + + if current_vel > self.target_vel_on_straight_line: + target_vel = self.target_vel_on_straight_line + sine - 1.0 + target_vel = max(target_vel, 0.05) + elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( + self.target_acc_on_straight_line + ): + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.5 * sine + ) + else: + target_vel = current_vel + abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + + # Adjust for deceleration based on achievement rate + if self.deceleration_rate - 0.05 <= achievement_rate < self.deceleration_rate: + target_vel = current_vel - abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + elif self.deceleration_rate <= achievement_rate: + target_vel = ( + (current_vel - self.params.a_max / acc_kp_of_pure_pursuit) + * (1.0 + 0.5 * sine) + * (1.0 - achievement_rate) + ) + + return target_vel + + def get_boundary_points(self): + self.boundary_points = np.vstack((self.A, self.B, self.C, self.D)) + + def check_in_boundary(self, current_position): + P = current_position[0:2] + + area_ABCD = computeTriangleArea(self.A, self.B, self.C) + computeTriangleArea( + self.C, self.D, self.A + ) + + area_PAB = computeTriangleArea(P, self.A, self.B) + area_PBC = computeTriangleArea(P, self.B, self.C) + area_PCD = computeTriangleArea(P, self.C, self.D) + area_PDA = computeTriangleArea(P, self.D, self.A) + + if area_PAB + area_PBC + area_PCD + area_PDA > area_ABCD * 1.001: + return False + else: + return True diff --git a/control_data_collecting_tool/scripts/courses/u_shaped.py b/control_data_collecting_tool/scripts/courses/u_shaped.py new file mode 100644 index 00000000..bca37ca5 --- /dev/null +++ b/control_data_collecting_tool/scripts/courses/u_shaped.py @@ -0,0 +1,232 @@ +#!/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. + +from courses.base_course import Base_Course +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor + + +def computeTriangleArea(A, B, C): + return 0.5 * abs(np.cross(B - A, C - A)) + + +def declare_u_shaped_return_params(node): + node.declare_parameter( + "velocity_on_curve", + 4.5, + ParameterDescriptor(description="Constant velocity on curve [m/s]"), + ) + + +class U_Shaped(Base_Course): + def __init__(self, step: float, param_dict): + super().__init__(step, param_dict) + + self.set_target_velocity_on_straight_line = False + self.target_vel_on_straight_line = 6.0 + self.target_acc_on_straight_line = 0.0 + self.vel_idx, self.acc_idx = 0, 0 + self.previous_part = "curve" + + self.deceleration_rate = 1.0 + + self.sine_period_for_velocity = 7.5 + self.velocity_on_curve = param_dict["velocity_on_curve"] + + def get_trajectory_points( + self, + long_side_length: float, + short_side_length: float, + ego_point=np.array([0.0, 0.0]), + goal_point=np.array([0.0, 0.0]), + ): + a = short_side_length + b = long_side_length + + # Boundary points between circular and linear trajectory + A = [-(b - a) / 2, a / 2] + B = [(b - a) / 2, a / 2] + C = [-(b - a) / 2, -a / 2] + D = [(b - a) / 2, -a / 2] + + R = a / 2 # radius of the circle + OL = [-(b - a) / 2, 0] # center of the left circle + OR = [(b - a) / 2, 0] # center of the right circle + + AB = b - a + arc_BD = np.pi * R + DC = b - a + arc_CA = np.pi * R + + total_distance = 2 * AB + 2 * np.pi * R + + t_array = np.arange(start=0.0, stop=total_distance, step=self.step).astype("float") + x = np.array([0.0 for i in range(len(t_array.copy()))]) + y = np.array([0.0 for i in range(len(t_array.copy()))]) + self.achievement_rates = t_array.copy() + self.parts = ["part" for _ in range(len(t_array.copy()))] + i_end = t_array.shape[0] + + for i, t in enumerate(t_array): + if t > AB + arc_BD + DC + arc_CA: + i_end = i + break + + if 0 <= t and t <= AB: + section_rate = t / AB + x[i] = section_rate * B[0] + (1 - section_rate) * A[0] + y[i] = section_rate * B[1] + (1 - section_rate) * A[1] + self.parts[i] = "straight" + self.achievement_rates[i] = section_rate + + if AB <= t and t <= AB + arc_BD: + section_rate = (t - AB) / arc_BD + x[i] = OR[0] + R * np.cos(np.pi / 2 - np.pi * section_rate) + y[i] = OR[1] + R * np.sin(np.pi / 2 - np.pi * section_rate) + self.parts[i] = "curve" + self.achievement_rates[i] = section_rate + + if AB + arc_BD <= t and t <= AB + arc_BD + DC: + section_rate = (t - AB - arc_BD) / DC + x[i] = section_rate * C[0] + (1 - section_rate) * D[0] + y[i] = section_rate * C[1] + (1 - section_rate) * D[1] + self.parts[i] = "straight" + self.achievement_rates[i] = section_rate + + if AB + arc_BD + DC <= t and t <= AB + arc_BD + DC + arc_CA: + section_rate = (t - AB - arc_BD - DC) / arc_CA + x[i] = OL[0] + R * np.cos(3 * np.pi / 2 - np.pi * section_rate) + y[i] = OL[1] + R * np.sin(3 * np.pi / 2 - np.pi * section_rate) + self.parts[i] = "curve" + self.achievement_rates[i] = section_rate + + # drop rest + x = x[:i_end] + y = y[:i_end] + self.trajectory_points = np.array([x, y]).T + + dx = (x[1:] - x[:-1]) / self.step + dy = (y[1:] - y[:-1]) / self.step + ddx = (dx[1:] - dx[:-1]) / self.step + ddy = (dy[1:] - dy[:-1]) / self.step + + self.yaw = np.arctan2(dy, dx) + self.yaw = np.array(self.yaw.tolist() + [self.yaw[-1]]) + self.curvature = ( + 1e-9 + abs(ddx * dy[:-1] - ddy * dx[:-1]) / (dx[:-1] ** 2 + dy[:-1] ** 2 + 1e-9) ** 1.5 + ) + self.curvature = np.array( + self.curvature.tolist() + [self.curvature[-2], self.curvature[-1]] + ) + + self.parts = self.parts[:i_end] + self.achievement_rates = self.achievement_rates[:i_end] + + return self.trajectory_points, self.yaw, self.curvature, self.parts, self.achievement_rates + + def get_target_velocity( + self, + nearestIndex, + current_time, + current_vel, + current_acc, + collected_data_counts_of_vel_acc, + collected_data_counts_of_vel_steer, + ): + part = self.parts[nearestIndex] + achievement_rate = self.achievement_rates[nearestIndex] + acc_kp_of_pure_pursuit = self.params.acc_kp + + # Check and update target velocity on straight line + if ( + (part == "straight" and self.previous_part == "curve") + or (part == "straight" and achievement_rate < 0.05) + ) and not self.set_target_velocity_on_straight_line: + self.acc_idx, self.vel_idx = self.choose_target_velocity_acc( + collected_data_counts_of_vel_acc + ) + self.target_acc_on_straight_line = self.params.a_bin_centers[self.acc_idx] + self.target_vel_on_straight_line = self.params.v_bin_centers[self.vel_idx] + + i = 0 + while self.parts[i + nearestIndex] == "straight": + i += 1 + + distance = i * self.step + stop_distance = self.target_vel_on_straight_line**2 / (2 * self.params.a_max) + self.deceleration_rate = 1.0 - stop_distance / distance + self.set_target_velocity_on_straight_line = True + + # Reset target velocity on line if entering a curve + if part == "curve": + self.set_target_velocity_on_straight_line = False + + self.previous_part = part + + # Calculate sine wave and apply to velocity + T = self.sine_period_for_velocity + sine = np.sin(2 * np.pi * current_time / T) * np.sin(np.pi * current_time / T) + + if current_vel > self.target_vel_on_straight_line: + target_vel = self.target_vel_on_straight_line + 1.5 * sine - 1.0 + target_vel = max(target_vel, 0.05) + elif current_vel < self.target_vel_on_straight_line - 2.0 * abs( + self.target_acc_on_straight_line + ): + target_vel = current_vel + self.params.a_max / acc_kp_of_pure_pursuit * ( + 1.25 + 0.5 * sine + ) + else: + target_vel = current_vel + abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + + # Adjust for deceleration based on achievement rate + if self.deceleration_rate - 0.05 <= achievement_rate < self.deceleration_rate: + target_vel = current_vel - abs( + self.target_acc_on_straight_line + ) / acc_kp_of_pure_pursuit * (1.25 + 0.5 * sine) + elif self.deceleration_rate <= achievement_rate: + target_vel = max( + current_vel - self.params.a_max / acc_kp_of_pure_pursuit * (1.0 + 0.5 * sine), + self.velocity_on_curve, + ) + + # Handle special conditions for curves or trajectory end + if part == "curve": + target_vel = self.velocity_on_curve + + return target_vel + + def get_boundary_points(self): + self.boundary_points = np.vstack((self.A, self.B, self.C, self.D)) + + def check_in_boundary(self, current_position): + P = current_position[0:2] + + area_ABCD = computeTriangleArea(self.A, self.B, self.C) + computeTriangleArea( + self.C, self.D, self.A + ) + + area_PAB = computeTriangleArea(P, self.A, self.B) + area_PBC = computeTriangleArea(P, self.B, self.C) + area_PCD = computeTriangleArea(P, self.C, self.D) + area_PDA = computeTriangleArea(P, self.D, self.A) + + if area_PAB + area_PBC + area_PCD + area_PDA > area_ABCD * 1.001: + return False + else: + return True diff --git a/control_data_collecting_tool/scripts/data_collecting_base_node.py b/control_data_collecting_tool/scripts/data_collecting_base_node.py index 885e6d7a..28d0a404 100644 --- a/control_data_collecting_tool/scripts/data_collecting_base_node.py +++ b/control_data_collecting_tool/scripts/data_collecting_base_node.py @@ -28,6 +28,12 @@ def __init__(self, node_name): super().__init__(node_name) # common params + self.declare_parameter( + "wheel_base", + 2.79, + ParameterDescriptor(description="Wheel base [m]"), + ) + self.declare_parameter( "NUM_BINS_V", 10, @@ -82,11 +88,8 @@ def __init__(self, node_name): ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), ) - self.declare_parameter( - "wheel_base", - 2.79, - ParameterDescriptor(description="Wheel base [m]"), - ) + self.ego_point = np.array([0.0, 0.0]) + self.goal_point = np.array([0.0, 0.0]) self.sub_odometry_ = self.create_subscription( Odometry, @@ -116,8 +119,8 @@ def __init__(self, node_name): 10, ) - self._present_kinematic_state = None - self._present_acceleration = None + self._present_kinematic_state = Odometry() + self._present_acceleration = AccelWithCovarianceStamped() self.present_operation_mode_ = None self._present_control_mode_ = None @@ -160,6 +163,12 @@ def __init__(self, node_name): def onOdometry(self, msg): self._present_kinematic_state = msg + self.ego_point = np.array( + [ + self._present_kinematic_state.pose.pose.position.x, + self._present_kinematic_state.pose.pose.position.y, + ] + ) def onAcceleration(self, msg): self._present_acceleration = msg diff --git a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py index c142744d..6b9e1efa 100755 --- a/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py +++ b/control_data_collecting_tool/scripts/data_collecting_pure_pursuit_trajectory_follower.py @@ -543,38 +543,38 @@ def control(self): # [3] publish marker marker_array = MarkerArray() - marker_traj = Marker() - marker_traj.type = 4 - marker_traj.id = 1 - marker_traj.header.frame_id = "map" + marker_trajectory = Marker() + marker_trajectory.type = 4 + marker_trajectory.id = 1 + marker_trajectory.header.frame_id = "map" - marker_traj.action = marker_traj.ADD + marker_trajectory.action = marker_trajectory.ADD - marker_traj.scale.x = 0.6 - marker_traj.scale.y = 0.0 - marker_traj.scale.z = 0.0 + marker_trajectory.scale.x = 0.6 + marker_trajectory.scale.y = 0.0 + marker_trajectory.scale.z = 0.0 - marker_traj.color.a = 1.0 - marker_traj.color.r = 0.0 - marker_traj.color.g = 1.0 - marker_traj.color.b = 0.0 + marker_trajectory.color.a = 1.0 + marker_trajectory.color.r = 0.0 + marker_trajectory.color.g = 1.0 + marker_trajectory.color.b = 0.0 - marker_traj.lifetime.nanosec = 500000000 - marker_traj.frame_locked = True + marker_trajectory.lifetime.nanosec = 500000000 + marker_trajectory.frame_locked = True - marker_traj.points = [] + marker_trajectory.points = [] tmp_marker_point = Point() tmp_marker_point.x = present_position[0] tmp_marker_point.y = present_position[1] tmp_marker_point.z = 0.0 - marker_traj.points.append(tmp_marker_point) + marker_trajectory.points.append(tmp_marker_point) tmp_marker_point = Point() tmp_marker_point.x = trajectory_position[targetIndex][0] tmp_marker_point.y = trajectory_position[targetIndex][1] tmp_marker_point.z = 0.0 - marker_traj.points.append(tmp_marker_point) + marker_trajectory.points.append(tmp_marker_point) - marker_array.markers.append(marker_traj) + marker_array.markers.append(marker_trajectory) self.data_collecting_lookahead_marker_array_pub_.publish(marker_array) # [99] debug plot diff --git a/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py index ac29dbc4..f6188c49 100755 --- a/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py +++ b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py @@ -35,29 +35,30 @@ class MessageWriter: def __init__(self, topics, node): self.topics = topics + self.subscribing_topic = [] + self.subscribed_topic = [] self.node = node self.message_writer = None + self.open_writer = False self.message_subscriptions_ = [] def create_writer(self): self.message_writer = SequentialWriter() def subscribe_topics(self): - topic_type_list = [] - unsubscribed_topic = [] + subscribable_topic_list = [] + subscribable_topic_type_list = [] + not_subscribed_topic = [] # Get topic types from the list of topics and store them for topic_name in self.topics: - topic_type = self.get_topic_type(topic_name) - topic_type_list.append(topic_type) - if topic_type is None: - # If a topic type is not found, mark it as unsubscribed - unsubscribed_topic.append(topic_name) - - # If some topics are not found, log a message and skip the subscription - if len(unsubscribed_topic) > 0: - self.node.get_logger().info(f"Failed to get type for topic: {unsubscribed_topic}") - return + if topic_name not in self.subscribed_topic: + topic_type = self.get_topic_type(topic_name) + if topic_type is not None: + subscribable_topic_list.append(topic_name) + subscribable_topic_type_list.append(topic_type) + else: + not_subscribed_topic.append(topic_name) # Generate a unique directory and filename based on the current time for the rosbag file now = datetime.now() @@ -68,20 +69,34 @@ def subscribe_topics(self): input_serialization_format="cdr", output_serialization_format="cdr" ) + # log not_subscribed topic + if len(not_subscribed_topic) > 0: + self.node.get_logger().info(f"Failed to subscribe to topics: {not_subscribed_topic}") + # Open the rosbag for writing - try: - self.message_writer.open(storage_options, converter_options) - except Exception as e: - self.node.get_logger().error(f"Failed to open bag: {e}") - return + if not self.open_writer: + try: + self.message_writer.open(storage_options, converter_options) + self.open_writer = True + except Exception as e: + self.node.get_logger().error(f"Failed to open bag: {e}") + self.message_writer = None + return # Create topics in the rosbag for recording - for topic_name, topic_type in zip(self.topics, topic_type_list): - self.node.get_logger().info(f"Recording topic: {topic_name} of type: {topic_type}") - topic_metadata = TopicMetadata( - name=topic_name, type=topic_type, serialization_format="cdr" - ) - self.message_writer.create_topic(topic_metadata) + if self.open_writer: + for topic_name, topic_type in zip( + subscribable_topic_list, subscribable_topic_type_list + ): + if topic_name not in self.subscribed_topic: + self.node.get_logger().info( + f"Recording topic: {topic_name} of type: {topic_type}" + ) + topic_metadata = TopicMetadata( + name=topic_name, type=topic_type, serialization_format="cdr" + ) + self.message_writer.create_topic(topic_metadata) + self.subscribing_topic.append(topic_name) def get_topic_type(self, topic_name): # Get the list of topics and their types from the ROS node @@ -91,9 +106,9 @@ def get_topic_type(self, topic_name): return types[0] return None - def start_record(self): + def record(self): # Subscribe to the topics and start recording messages - for topic_name in self.topics: + for topic_name in self.subscribing_topic: topic_type = self.get_topic_type(topic_name) if topic_type: msg_module = get_message(topic_type) @@ -101,7 +116,11 @@ def start_record(self): msg_module, topic_name, partial(self.callback_write_message, topic_name), 10 ) self.message_subscriptions_.append(subscription_) - self.node.get_logger().info("start recording rosbag") + self.subscribed_topic.append(topic_name) + + for topic_name in self.subscribed_topic: + if topic_name in self.subscribing_topic: + self.subscribing_topic.remove(topic_name) # call back function called in start recording def callback_write_message(self, topic_name, message): @@ -116,8 +135,11 @@ def stop_record(self): # Stop recording by destroying the subscriptions and deleting the writer for subscription_ in self.message_subscriptions_: self.node.destroy_subscription(subscription_) - del self.message_writer - self.node.get_logger().info("stop recording rosbag") + self.message_writer = None + self.subscribing_topic = [] + self.subscribed_topic = [] + self.open_writer = False + self.node.get_logger().info("Stop recording rosbag") class DataCollectingRosbagRecord(Node): @@ -166,10 +188,8 @@ def record_message(self): 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(LOCAL) @@ -177,20 +197,16 @@ def record_message(self): 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 + self.writer.subscribe_topics() + self.writer.record() # 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.present_operation_mode_ != 3 or self._present_control_mode_ != 1 + ) and self.subscribed: self.writer.stop_record() self.subscribed = False - self.recording = False def main(args=None): diff --git a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py index cbc86d47..b2455490 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -16,14 +16,15 @@ from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint +from courses.load_course import declare_course_params +from courses.load_course import load_course from data_collecting_base_node import DataCollectingBaseNode from geometry_msgs.msg import Point from geometry_msgs.msg import PolygonStamped +from geometry_msgs.msg import PoseStamped import matplotlib.pyplot as plt import numpy as np -from numpy import cos from numpy import pi -from numpy import sin from rcl_interfaces.msg import ParameterDescriptor import rclpy from scipy.spatial.transform import Rotation as R @@ -52,227 +53,6 @@ def getYaw(orientation_xyzw): return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2] -def computeTriangleArea(A, B, C): - return 0.5 * abs(np.cross(B - A, C - A)) - - -def get_eight_course_trajectory_points( - long_side_length: float, - short_side_length: float, - step: float, -): - a = short_side_length - b = long_side_length - - C = [-(b / 2 - (1.0 - np.sqrt(3) / 2) * a), -a / 2] - D = [(b / 2 - (1.0 - np.sqrt(3) / 2) * a), -a / 2] - - R = a # radius of the circle - OL = [-b / 2 + a, 0] # center of the left circle - OR = [b / 2 - a, 0] # center of the right circle - OB = np.sqrt( - (b / 2 + (1.0 - np.sqrt(3) / 2) * a) ** 2 + (a / 2) ** 2 - ) # half length of the linear trajectory - AD = 2 * OB - θB = np.arctan( - a / 2 / (b / 2 + (1.0 - np.sqrt(3) / 2) * a) - ) # Angle that OB makes with respect to x-axis - BD = 2 * np.pi * R / 6 # the length of arc BD - AC = BD - CO = OB - - total_distance = 4 * OB + 2 * BD - - t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") - x = [0.0 for i in range(len(t_array.copy()))] - y = [0.0 for i in range(len(t_array.copy()))] - yaw = t_array.copy() - curve = t_array.copy() - achievement_rates = t_array.copy() - parts = ["part" for _ in range(len(t_array.copy()))] - i_end = t_array.shape[0] - - for i, t in enumerate(t_array): - if t > OB + BD + AD + AC + CO: - i_end = i - break - - if 0 <= t and t <= OB: - x[i] = (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t / OB - y[i] = a * t / (2 * OB) - yaw[i] = θB - curve[i] = 1e-10 - parts[i] = "linear_positive" - achievement_rates[i] = t / (2 * OB) + 0.5 - - if OB <= t and t <= OB + BD: - t1 = t - OB - t1_rad = t1 / R - x[i] = OR[0] + R * np.cos(np.pi / 6 - t1_rad) - y[i] = OR[1] + R * np.sin(np.pi / 6 - t1_rad) - yaw[i] = -t1_rad - curve[i] = 1 / R - parts[i] = "right_circle" - achievement_rates[i] = t1 / BD - - if OB + BD <= t and t <= OB + BD + AD: - t2 = t - (OB + BD) - x[i] = D[0] - (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t2 / OB - y[i] = D[1] + a * t2 / (2 * OB) - yaw[i] = np.pi - θB - curve[i] = 1e-10 - parts[i] = "linear_negative" - achievement_rates[i] = t2 / (2 * OB) - - if OB + BD + AD <= t and t <= OB + BD + AD + AC: - t3 = t - (OB + BD + AD) - t3_rad = t3 / R - x[i] = OL[0] - R * np.cos(-np.pi / 6 + t3_rad) - y[i] = OL[1] - R * np.sin(-np.pi / 6 + t3_rad) - yaw[i] = np.pi + t3_rad - curve[i] = 1 / R - parts[i] = "left_circle" - achievement_rates[i] = t3 / BD - - if OB + BD + AD + AC <= t and t <= OB + BD + AD + AC + CO: - t4 = t - (OB + BD + AD + AC) - x[i] = C[0] + (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t4 / OB - y[i] = C[1] + a * t4 / (2 * OB) - yaw[i] = θB - curve[i] = 1e-10 - parts[i] = "linear_positive" - achievement_rates[i] = t4 / (2 * OB) - - # drop rest - x = x[:i_end] - y = y[:i_end] - yaw = yaw[:i_end] - curve = curve[:i_end] - parts = parts[:i_end] - achievement_rates = achievement_rates[:i_end] - - if USE_CURVATURE_RADIUS_FLAG: - return np.array([x, y]).T, yaw, 1 / curve[:i_end], parts, achievement_rates - else: - return np.array([x, y]).T, yaw, curve[:i_end], parts, achievement_rates - - -def get_straight_line_course_trajectory_points( - long_side_length: float, short_side_length: float, step: float, COURSE_NAME -): - b = long_side_length - - total_distance = b - t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") - - if COURSE_NAME == "straight_line_positive": - yaw = np.zeros(len(t_array)) - parts = ["linear_positive" for _ in range(len(t_array.copy()))] - x = np.linspace(-total_distance / 2, total_distance / 2, len(t_array)) - y = np.zeros(len(t_array)) - - elif COURSE_NAME == "straight_line_negative": - yaw = pi * np.ones(len(t_array)) - parts = ["linear_negative" for _ in range(len(t_array.copy()))] - x = np.linspace(total_distance / 2, -total_distance / 2, len(t_array)) - y = np.zeros(len(t_array)) - - curve = 1e-9 * np.ones(len(t_array)) - achievement_rates = np.linspace(0.0, 1.0, len(t_array)) - - if USE_CURVATURE_RADIUS_FLAG: - return np.vstack((x, y)).T, yaw, 1 / curve, parts, achievement_rates - else: - return np.vstack((x, y)).T, yaw, curve, parts, achievement_rates - - -def get_u_shaped_return_course_trajectory_points( - long_side_length: float, short_side_length: float, step: float -): - a = short_side_length - b = long_side_length - - # Boundary points between circular and linear trajectory - A = [-(b - a) / 2, a / 2] - B = [(b - a) / 2, a / 2] - C = [-(b - a) / 2, -a / 2] - D = [(b - a) / 2, -a / 2] - - R = a / 2 # radius of the circle - OL = [-(b - a) / 2, 0] # center of the left circle - OR = [(b - a) / 2, 0] # center of the right circle - - AB = b - a - arc_BD = pi * R - DC = b - a - arc_CA = pi * R - - total_distance = 2 * AB + 2 * np.pi * R - - t_array = np.arange(start=0.0, stop=total_distance, step=step).astype("float") - x = [0.0 for i in range(len(t_array.copy()))] - y = [0.0 for i in range(len(t_array.copy()))] - yaw = t_array.copy() - curve = t_array.copy() - achievement_rates = t_array.copy() - parts = ["part" for _ in range(len(t_array.copy()))] - i_end = t_array.shape[0] - - for i, t in enumerate(t_array): - if t > AB + arc_BD + DC + arc_CA: - i_end = i - break - - if 0 <= t and t <= AB: - section_rate = t / AB - x[i] = section_rate * B[0] + (1 - section_rate) * A[0] - y[i] = section_rate * B[1] + (1 - section_rate) * A[1] - yaw[i] = 0.0 - curve[i] = 1e-10 - parts[i] = "linear_positive" - achievement_rates[i] = section_rate - - if AB <= t and t <= AB + arc_BD: - section_rate = (t - AB) / arc_BD - x[i] = OR[0] + R * cos(pi / 2 - pi * section_rate) - y[i] = OR[1] + R * sin(pi / 2 - pi * section_rate) - yaw[i] = pi / 2 - pi * section_rate - pi / 2 - curve[i] = 1.0 / R - parts[i] = "right_circle" - achievement_rates[i] = section_rate - - if AB + arc_BD <= t and t <= AB + arc_BD + DC: - section_rate = (t - AB - arc_BD) / DC - x[i] = section_rate * C[0] + (1 - section_rate) * D[0] - y[i] = section_rate * C[1] + (1 - section_rate) * D[1] - yaw[i] = pi - curve[i] = 1e-10 - parts[i] = "linear_negative" - achievement_rates[i] = section_rate - - if AB + arc_BD + DC <= t and t <= AB + arc_BD + DC + arc_CA: - section_rate = (t - AB - arc_BD - DC) / arc_CA - x[i] = OL[0] + R * cos(3 * pi / 2 - pi * section_rate) - y[i] = OL[1] + R * sin(3 * pi / 2 - pi * section_rate) - yaw[i] = 3 * pi / 2 - pi * section_rate - pi / 2 - curve[i] = 1.0 / R - parts[i] = "left_circle" - achievement_rates[i] = section_rate - - # drop rest - x = x[:i_end] - y = y[:i_end] - yaw = yaw[:i_end] - curve = curve[:i_end] - parts = parts[:i_end] - achievement_rates = achievement_rates[:i_end] - - if USE_CURVATURE_RADIUS_FLAG: - return np.array([x, y]).T, yaw, 1 / curve[:i_end], parts, achievement_rates - else: - return np.array([x, y]).T, yaw, curve[:i_end], parts, achievement_rates - - # inherits from DataCollectingBaseNode class DataCollectingTrajectoryPublisher(DataCollectingBaseNode): def __init__(self): @@ -282,9 +62,11 @@ def __init__(self): "COURSE_NAME", "eight_course", ParameterDescriptor( - description="Course name [eight_course, u_shaped_return, straight_line_positive, straight_line_negative]" + description="Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`, `reversal_loop_circle`, `along_road`]" ), ) + # set course name + self.COURSE_NAME = self.get_parameter("COURSE_NAME").value self.declare_parameter( "acc_kp", @@ -382,6 +164,11 @@ def __init__(self): ParameterDescriptor(description="Maximum velocity for data collection [m/ss]"), ) + """ + Declare course specific parameters + """ + declare_course_params(self.COURSE_NAME, self) + self.trajectory_for_collecting_data_pub_ = self.create_publisher( Trajectory, "/data_collecting_trajectory", @@ -407,39 +194,40 @@ def __init__(self): 1, ) self.sub_data_collecting_area_ + self._data_collecting_area_polygon = None - # set course name - self.COURSE_NAME = self.get_parameter("COURSE_NAME").value - - self.trajectory_parts = None - self.trajectory_achievement_rates = None - - self.prev_part = "left_circle" - - self.acc_idx = 0 - self.vel_idx = 0 + self.sub_data_collecting_gaol_pose_ = self.create_subscription( + PoseStamped, + "/data_collecting_goal_pose", + self.onGoalPose, + 1, + ) + self.sub_data_collecting_area_ - self.target_acc_on_line = 0.0 - self.target_vel_on_line = 0.0 + # obtain ros params as dictionary + param_names = self._parameters + params = self.get_parameters(param_names) + params_dict = {param.name: param.value for param in params} - self.on_line_vel_flag = True - self.deceleration_rate = 0.6 + self.trajectory_step = 0.1 + self.course = load_course(self.COURSE_NAME, self.trajectory_step, params_dict) self.timer_period_callback = 0.03 # 30ms - self.traj_step = 0.1 - self.timer_traj = self.create_timer(self.timer_period_callback, self.timer_callback_traj) - - if debug_matplotlib_plot_flag: - self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) - plt.ion() - - self._present_kinematic_state = None - self._present_acceleration = None + self.timer_trajectory_generator = self.create_timer( + self.timer_period_callback, self.callback_trajectory_generator + ) self.trajectory_position_data = None self.trajectory_yaw_data = None self.trajectory_longitudinal_velocity_data = None self.trajectory_curvature_data = None + self.trajectory_parts = None + self.trajectory_achievement_rates = None + + self.nearestIndex = 0 + self.yaw_offset = 0.0 + self.rectangle_center_position = np.zeros(2) + self.current_target_longitudinal_velocity = ( self.get_parameter("target_longitudinal_velocity").get_parameter_value().double_value ) @@ -447,29 +235,9 @@ def __init__(self): self.get_parameter("mov_ave_window").get_parameter_value().integer_value ) - self.one_round_progress_rate = None self.vel_noise_list = [] - collecting_data_min_v, collecting_data_max_v = ( - self.get_parameter("COLLECTING_DATA_V_MIN").get_parameter_value().double_value, - self.get_parameter("COLLECTING_DATA_V_MAX").get_parameter_value().double_value, - ) - - collecting_data_min_a, collecting_data_max_a = ( - self.get_parameter("COLLECTING_DATA_A_MIN").get_parameter_value().double_value, - self.get_parameter("COLLECTING_DATA_A_MAX").get_parameter_value().double_value, - ) - - self.collecting_data_min_n_v = max([np.digitize(collecting_data_min_v, self.v_bins) - 1, 0]) - self.collecting_data_max_n_v = ( - min([np.digitize(collecting_data_max_v, self.v_bins) - 1, self.num_bins_v - 1]) + 1 - ) - - self.collecting_data_min_n_a = max([np.digitize(collecting_data_min_a, self.v_bins) - 1, 0]) - self.collecting_data_max_n_a = ( - min([np.digitize(collecting_data_max_a, self.v_bins) - 1, self.num_bins_a - 1]) + 1 - ) - + # subscriptions of data counter self.collected_data_counts_of_vel_acc_subscription_ = self.create_subscription( Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_acc", @@ -486,6 +254,10 @@ def __init__(self): ) self.collected_data_counts_of_vel_steer_subscription_ + if debug_matplotlib_plot_flag: + self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) + plt.ion() + def subscribe_collected_data_counts_of_vel_acc(self, msg): rows = msg.layout.dim[0].size cols = msg.layout.dim[1].size @@ -500,187 +272,36 @@ def onDataCollectingArea(self, msg): self._data_collecting_area_polygon = msg self.updateNominalTargetTrajectory() - def get_target_velocity(self, nearestIndex): - part = self.trajectory_parts[ - nearestIndex - ] # "left_circle", "right_circle", "linear_positive", "linear_negative" - achievement_rate = self.trajectory_achievement_rates[nearestIndex] - current_vel = self._present_kinematic_state.twist.twist.linear.x + def onGoalPose(self, msg): + self.goal_point[0] = msg.pose.position.x + self.goal_point[1] = msg.pose.position.y - acc_kp_of_pure_pursuit = self.get_parameter("acc_kp").get_parameter_value().double_value - N_V = self.num_bins_v - N_A = self.num_bins_a + self.trajectory_position_data = None + self.trajectory_yaw_data = None + self.updateNominalTargetTrajectory() - max_lateral_accel = ( - self.get_parameter("max_lateral_accel").get_parameter_value().double_value - ) - if USE_CURVATURE_RADIUS_FLAG: - max_vel_from_lateral_acc = np.sqrt( - max_lateral_accel * self.trajectory_curvature_data[nearestIndex] + def updateNominalTargetTrajectory(self): + if self._data_collecting_area_polygon is not None: + data_collecting_area = np.array( + [ + np.array( + [ + self._data_collecting_area_polygon.polygon.points[i].x, + self._data_collecting_area_polygon.polygon.points[i].y, + self._data_collecting_area_polygon.polygon.points[i].z, + ] + ) + for i in range(4) + ] ) else: - max_vel_from_lateral_acc = np.sqrt( - max_lateral_accel / self.trajectory_curvature_data[nearestIndex] - ) - - target_vel = min([self.v_max / N_V, max_vel_from_lateral_acc]) - - # set self.target_acc_on_line and self.target_vel_on_line after vehicle from circle part to linear part - min_data_num_margin = 5 - min_index_list = [] - if ( - (self.prev_part == "left_circle" or self.prev_part == "right_circle") - and (part == "linear_positive" or part == "linear_negative") - or ( - (part == "linear_positive" or part == "linear_negative") and achievement_rate < 0.05 - ) - ): - self.on_line_vel_flag = True - min_num_data = 1e12 - - # do not collect data when velocity and acceleration are low - exclude_idx_list = [(0, 0), (1, 0), (2, 0), (0, 1), (1, 1), (0, 2)] - # do not collect data when velocity and acceleration are high - exclude_idx_list += [ - (-1 + N_V, -1 + N_A), - (-2 + N_V, -1 + N_A), - (-3 + N_V, -1 + N_A), - (-1 + N_V, -2 + N_A), - (-2 + N_V, -2 + N_A), - (-1 + N_V, -3 + N_A), - ] - - for i in range(self.collecting_data_min_n_v, self.collecting_data_max_n_v): - for j in range(self.collecting_data_min_n_a, self.collecting_data_max_n_a): - if (i, j) not in exclude_idx_list: - if ( - min_num_data - min_data_num_margin - > self.collected_data_counts_of_vel_acc[i, j] - ): - min_num_data = self.collected_data_counts_of_vel_acc[i, j] - min_index_list.clear() - min_index_list.append((j, i)) - - elif ( - min_num_data + min_data_num_margin - > self.collected_data_counts_of_vel_acc[i, j] - ): - min_index_list.append((j, i)) - - self.acc_idx, self.vel_idx = min_index_list[np.random.randint(0, len(min_index_list))] - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - self.target_vel_on_line = self.v_bin_centers[self.vel_idx] - - if ( - self.COURSE_NAME == "u_shaped_return" - or self.COURSE_NAME == "straight_line_positive" - or self.COURSE_NAME == "straight_line_negative" - ): - if self.target_vel_on_line > self.v_max * 3.0 / 4.0: - self.deceleration_rate = 0.55 + 0.10 - elif self.target_vel_on_line > self.v_max / 2.0: - self.deceleration_rate = 0.65 + 0.10 - else: - self.deceleration_rate = 0.85 + 0.10 - - elif self.COURSE_NAME == "eight_course": - if self.target_vel_on_line > self.v_max * 3.0 / 4.0: - self.deceleration_rate = 0.55 - elif self.target_vel_on_line > self.v_max / 2.0: - self.deceleration_rate = 0.65 - else: - self.deceleration_rate = 0.85 + data_collecting_area = np.array([np.array([i, i**2]) for i in range(4)]) - # set target velocity on linear part - if part == "linear_positive" or part == "linear_negative": - if ( - current_vel > self.target_vel_on_line - self.v_max / N_V / 8.0 - and self.target_vel_on_line >= self.v_max / 2.0 - ): - self.on_line_vel_flag = False - elif ( - abs(current_vel - self.target_vel_on_line) < self.v_max / N_V / 4.0 - and self.target_vel_on_line < self.v_max / 2.0 - ): - self.on_line_vel_flag = False - - # accelerate until vehicle reaches target_vel_on_line - if 0.0 <= achievement_rate and achievement_rate < 0.45 and self.on_line_vel_flag: - target_vel = self.target_vel_on_line - - if ( - current_vel > self.target_vel_on_line - self.v_max / N_V * 0.5 - and self.target_acc_on_line > 2.0 * self.a_max / N_A - ): - target_vel = current_vel + self.target_acc_on_line / acc_kp_of_pure_pursuit - - # collect target_acceleration data when current velocity is close to target_vel_on_line - elif ( - achievement_rate < self.deceleration_rate - or self.target_vel_on_line < self.v_max / 2.0 - ): - if self.collected_data_counts_of_vel_acc[self.vel_idx, self.acc_idx] > 50: - self.acc_idx = np.argmin(self.collected_data_counts_of_vel_acc[self.vel_idx, :]) - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - - if ( - current_vel - < max( - [self.target_vel_on_line - 1.5 * self.v_max / N_V, self.v_max / N_V / 2.0] - ) - and self.target_acc_on_line < 0.0 - ): - self.acc_idx = np.argmin( - self.collected_data_counts_of_vel_acc[self.vel_idx, int(N_A / 2.0) : N_A] - ) + int(N_A / 2) - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - - elif ( - current_vel > self.target_vel_on_line + 1.5 * self.v_max / N_V - and self.target_acc_on_line > 0.0 - ): - self.acc_idx = np.argmin( - self.collected_data_counts_of_vel_acc[self.vel_idx, 0 : int(N_A / 2.0)] - ) - self.target_acc_on_line = self.a_bin_centers[self.acc_idx] - - target_vel = current_vel + self.target_acc_on_line / acc_kp_of_pure_pursuit - - # deceleration - if self.deceleration_rate <= achievement_rate: - if self.COURSE_NAME == "eight_course" or self.COURSE_NAME == "u_shaped_return": - target_vel = np.min([self.v_max / 10.0, max_vel_from_lateral_acc / 2.0]) - elif ( - self.COURSE_NAME == "straight_line_positive" - or self.COURSE_NAME == "straight_line_negative" - ): - target_vel = 0.0 - - # set target velocity on circle part - if part == "left_circle" or part == "right_circle": - if achievement_rate < 0.10 and self.target_vel_on_line > self.v_max / 2.0: - target_vel = np.min([self.v_max / 10.0, max_vel_from_lateral_acc / 2.0]) - elif achievement_rate < 0.50: - target_vel = max_vel_from_lateral_acc / 2.0 - else: - target_vel = max_vel_from_lateral_acc - - self.prev_part = part - - return target_vel - - def updateNominalTargetTrajectory(self): - data_collecting_area = np.array( - [ - np.array( - [ - self._data_collecting_area_polygon.polygon.points[i].x, - self._data_collecting_area_polygon.polygon.points[i].y, - self._data_collecting_area_polygon.polygon.points[i].z, - ] - ) - for i in range(4) - ] + A, B, C, D = ( + data_collecting_area[0][0:2], + data_collecting_area[1][0:2], + data_collecting_area[2][0:2], + data_collecting_area[3][0:2], ) # [1] compute an approximate rectangle @@ -708,7 +329,7 @@ def updateNominalTargetTrajectory(self): ) # [2] compute whole trajectory - # [2-1] generate figure eight path + # [2-1] generate path if la > lb: long_side_length = la short_side_length = lb @@ -721,6 +342,7 @@ def updateNominalTargetTrajectory(self): vec_long_side = ( unit_vec_from_center_to_point0_data + unit_vec_from_center_to_point1_data ) + unit_vec_long_side = vec_long_side / np.sqrt((vec_long_side**2).sum()) if unit_vec_long_side[1] < 0: unit_vec_long_side *= -1 @@ -734,180 +356,63 @@ def updateNominalTargetTrajectory(self): actual_long_side = max(long_side_length - long_side_margin, 1.1) actual_short_side = max(short_side_length - long_side_margin, 1.0) - if self.COURSE_NAME == "eight_course": - ( - trajectory_position_data, - trajectory_yaw_data, - trajectory_curvature_data, - self.trajectory_parts, - self.trajectory_achievement_rates, - ) = get_eight_course_trajectory_points( - actual_long_side, actual_short_side, self.traj_step - ) + self.yaw_offset = yaw_offset + self.rectangle_center_position = rectangle_center_position - elif ( - self.COURSE_NAME == "straight_line_positive" - or self.COURSE_NAME == "straight_line_negative" - ): - ( - trajectory_position_data, - trajectory_yaw_data, - trajectory_curvature_data, - self.trajectory_parts, - self.trajectory_achievement_rates, - ) = get_straight_line_course_trajectory_points( - actual_long_side, actual_short_side, self.traj_step, self.COURSE_NAME - ) + if self.COURSE_NAME is not None: + self.course.set_vertices(A, B, C, D) - elif self.COURSE_NAME == "u_shaped_return": + self.course.get_trajectory_points( + actual_long_side, actual_short_side, self.ego_point, self.goal_point + ) ( - trajectory_position_data, - trajectory_yaw_data, - trajectory_curvature_data, + self.trajectory_position_data, + self.trajectory_yaw_data, + self.trajectory_curvature_data, self.trajectory_parts, self.trajectory_achievement_rates, - ) = get_u_shaped_return_course_trajectory_points( - actual_long_side, actual_short_side, self.traj_step + ) = self.course.return_trajectory_points( + self.yaw_offset, self.rectangle_center_position ) + self.course.get_boundary_points() + self.boundary_points = self.course.boundary_points + else: self.trajectory_position_data = None self.trajectory_yaw_data = None self.trajectory_longitudinal_velocity_data = None self.trajectory_curvature_data = None - for i in range(len(trajectory_yaw_data)): - if trajectory_yaw_data[i] > np.pi: - trajectory_yaw_data[i] -= 2 * np.pi - if trajectory_yaw_data[i] < -np.pi: - trajectory_yaw_data[i] += 2 * np.pi - - # [2-2] translation and rotation of origin - rot_matrix = np.array( - [ - [np.cos(yaw_offset), -np.sin(yaw_offset)], - [np.sin(yaw_offset), np.cos(yaw_offset)], - ] - ) - trajectory_position_data = (rot_matrix @ trajectory_position_data.T).T - trajectory_position_data += rectangle_center_position - trajectory_yaw_data += yaw_offset - - # [2-3] smoothing figure eight path - window = self.get_parameter("mov_ave_window").get_parameter_value().integer_value - self.current_window = 1 * window - if window < len(trajectory_position_data): - w = np.ones(window) / window - augmented_position_data = np.vstack( - [ - trajectory_position_data[-window:], - trajectory_position_data, - trajectory_position_data[:window], - ] - ) - trajectory_position_data[:, 0] = ( - 1 * np.convolve(augmented_position_data[:, 0], w, mode="same")[window:-window] - ) - trajectory_position_data[:, 1] = ( - 1 * np.convolve(augmented_position_data[:, 1], w, mode="same")[window:-window] - ) - augmented_yaw_data = np.hstack( - [ - trajectory_yaw_data[-window:], - trajectory_yaw_data, - trajectory_yaw_data[:window], - ] - ) - smoothed_trajectory_yaw_data = trajectory_yaw_data.copy() - for i in range(len(trajectory_yaw_data)): - tmp_yaw = trajectory_yaw_data[i] - tmp_data = ( - augmented_yaw_data[window + (i - window // 2) : window + (i + window // 2)] - - tmp_yaw - ) - for j in range(len(tmp_data)): - if tmp_data[j] > np.pi: - tmp_data[j] -= 2 * np.pi - if tmp_data[j] < -np.pi: - tmp_data[j] += 2 * np.pi - tmp_data = np.convolve(tmp_data, w, mode="same") - smoothed_trajectory_yaw_data[i] = ( - tmp_yaw + np.convolve(tmp_data, w, mode="same")[window // 2] - ) - if smoothed_trajectory_yaw_data[i] > np.pi: - smoothed_trajectory_yaw_data[i] -= 2 * np.pi - if smoothed_trajectory_yaw_data[i] < -np.pi: - smoothed_trajectory_yaw_data[i] += 2 * np.pi - - trajectory_yaw_data = smoothed_trajectory_yaw_data.copy() - - if not USE_CURVATURE_RADIUS_FLAG: - augmented_curvature_data = np.hstack( - [ - trajectory_curvature_data[-window:], - trajectory_curvature_data, - trajectory_curvature_data[:window], - ] - ) - trajectory_curvature_data = ( - 1 * np.convolve(augmented_curvature_data, w, mode="same")[window:-window] - ) - # [2-4] nominal velocity - target_longitudinal_velocity = ( - self.get_parameter("target_longitudinal_velocity").get_parameter_value().double_value - ) - - trajectory_longitudinal_velocity_data = target_longitudinal_velocity * np.zeros( - len(trajectory_position_data) - ) - self.current_target_longitudinal_velocity = 1 * target_longitudinal_velocity - - self.trajectory_position_data = trajectory_position_data.copy() - self.trajectory_yaw_data = trajectory_yaw_data.copy() - self.trajectory_longitudinal_velocity_data = trajectory_longitudinal_velocity_data.copy() - self.trajectory_curvature_data = trajectory_curvature_data.copy() - - self.get_logger().info("update nominal target trajectory") - - def checkInDateCollectingArea(self, current_pos): - data_collecting_area = np.array( - [ - np.array( - [ - self._data_collecting_area_polygon.polygon.points[i].x, - self._data_collecting_area_polygon.polygon.points[i].y, - self._data_collecting_area_polygon.polygon.points[i].z, - ] - ) - for i in range(4) - ] - ) + # [2-2] nominal velocity - A, B, C, D = ( - data_collecting_area[0][0:2], - data_collecting_area[1][0:2], - data_collecting_area[2][0:2], - data_collecting_area[3][0:2], - ) - P = current_pos[0:2] - - area_ABCD = computeTriangleArea(A, B, C) + computeTriangleArea(C, D, A) + if self.trajectory_position_data is not None: + target_longitudinal_velocity = ( + self.get_parameter("target_longitudinal_velocity") + .get_parameter_value() + .double_value + ) - area_PAB = computeTriangleArea(P, A, B) - area_PBC = computeTriangleArea(P, B, C) - area_PCD = computeTriangleArea(P, C, D) - area_PDA = computeTriangleArea(P, D, A) + self.trajectory_longitudinal_velocity_data = target_longitudinal_velocity * np.zeros( + len(self.trajectory_position_data) + ) + self.current_target_longitudinal_velocity = 1 * target_longitudinal_velocity - if area_PAB + area_PBC + area_PCD + area_PDA > area_ABCD * 1.001: - return False + if self.trajectory_position_data is not None: + self.nearestIndex = 0 + self.get_logger().info("update nominal target trajectory") else: - return True + self.get_logger().error("Fail to generate trajectory") - def timer_callback_traj(self): + def callback_trajectory_generator(self): if ( - self._present_kinematic_state is not None - and self._present_acceleration is not None - and self.trajectory_position_data is not None + ( + self._present_kinematic_state.pose is not None + and self._present_acceleration.accel is not None + and self.trajectory_position_data is not None + ) + or self.trajectory_position_data is not None + or self.trajectory_yaw_data is not None ): # [0] update nominal target trajectory if changing related ros2 params target_longitudinal_velocity = ( @@ -933,6 +438,7 @@ def timer_callback_traj(self): self._present_kinematic_state.pose.pose.position.z, ] ) + present_orientation = np.array( [ self._present_kinematic_state.pose.pose.orientation.x, @@ -952,7 +458,7 @@ def timer_callback_traj(self): if np.linalg.norm(present_orientation) < 1e-6: present_yaw = self.previous_yaw else: - present_yaw = getYaw(present_orientation) + present_yaw = getYaw(present_orientation)[0] self.previous_yaw = present_yaw # [2] get whole trajectory data @@ -998,56 +504,52 @@ def timer_callback_traj(self): self.vel_noise_list.pop(0) # [4] find near point index for local trajectory - distance = np.sqrt(((trajectory_position_data - present_position[:2]) ** 2).sum(axis=1)) - index_array_near = np.argsort(distance) - - nearestIndex = None - if (self.one_round_progress_rate is None) or (present_linear_velocity[0] < 0.1): - # if initializing, or if re-initialize while stopping - nearestIndex = index_array_near[0] + if self.course.closed: + index_range = np.arange( + self.nearestIndex - len(trajectory_position_data) // 4, + self.nearestIndex + len(trajectory_position_data) // 4, + ) % len(trajectory_position_data) else: - for i in range(len(index_array_near)): - progress_rate_diff = ( - 1.0 * index_array_near[i] / len(trajectory_position_data) - ) - self.one_round_progress_rate - if progress_rate_diff > 0.5: - progress_rate_diff -= 1.0 - if progress_rate_diff < -0.5: - progress_rate_diff += 1.0 - near_progress_rate_threshold = 0.2 - if np.abs(progress_rate_diff) < near_progress_rate_threshold: - nearestIndex = 1 * index_array_near[i] - break - if nearestIndex is None: - nearestIndex = index_array_near[0] - - self.one_round_progress_rate = 1.0 * nearestIndex / len(trajectory_position_data) - + min_index_range = max(0, self.nearestIndex - len(trajectory_position_data) // 4) + max_index_range = min( + len(trajectory_position_data), + self.nearestIndex + len(trajectory_position_data) // 4, + ) + index_range = np.arange(min_index_range, max_index_range) + distance = np.sqrt( + ((trajectory_position_data[index_range] - present_position[:2]) ** 2).sum(axis=1) + ) + index_array_near = np.argsort(distance) + self.nearestIndex = index_range[index_array_near[0]] # set target velocity - target_vel = self.get_target_velocity(nearestIndex) + present_vel = present_linear_velocity[0] + present_acc = self._present_acceleration.accel.accel.linear.x + current_time = self.get_clock().now().nanoseconds / 1e9 + target_vel = self.course.get_target_velocity( + self.nearestIndex, + current_time, + present_vel, + present_acc, + self.collected_data_counts_of_vel_acc, + self.collected_data_counts_of_vel_steer, + ) trajectory_longitudinal_velocity_data = np.array( - [target_vel for _ in range(len(trajectory_longitudinal_velocity_data))] + [target_vel for _ in range(len(trajectory_position_data))] ) # [5] modify target velocity # [5-1] add noise aug_data_length = len(trajectory_position_data) // 4 - trajectory_position_data = np.vstack( - [trajectory_position_data, trajectory_position_data[:aug_data_length]] - ) - trajectory_yaw_data = np.hstack( - [trajectory_yaw_data, trajectory_yaw_data[:aug_data_length]] - ) trajectory_longitudinal_velocity_data = np.hstack( [ trajectory_longitudinal_velocity_data, trajectory_longitudinal_velocity_data[:aug_data_length], ] ) - trajectory_longitudinal_velocity_data[nearestIndex:] += np.array(self.vel_noise_list)[ - : len(trajectory_longitudinal_velocity_data[nearestIndex:]) - ] + trajectory_longitudinal_velocity_data[self.nearestIndex :] += np.array( + self.vel_noise_list + )[: len(trajectory_longitudinal_velocity_data[self.nearestIndex :])] trajectory_longitudinal_velocity_data_without_limit = ( trajectory_longitudinal_velocity_data.copy() ) @@ -1076,6 +578,7 @@ def timer_callback_traj(self): trajectory_longitudinal_velocity_data = np.minimum( trajectory_longitudinal_velocity_data, lateral_acc_limit ) + # [5-3] apply limit by lateral error velocity_limit_by_tracking_error = ( self.get_parameter("velocity_limit_by_tracking_error") @@ -1092,10 +595,12 @@ def timer_callback_traj(self): ) tmp_lateral_error = np.sqrt( - ((trajectory_position_data[nearestIndex] - present_position[:2]) ** 2).sum() + ((trajectory_position_data[self.nearestIndex] - present_position[:2]) ** 2).sum() ) - tmp_yaw_error = np.abs(present_yaw - trajectory_yaw_data[nearestIndex]) + tmp_yaw_error = np.abs( + (present_yaw - trajectory_yaw_data[self.nearestIndex] + np.pi) % (2 * np.pi) - np.pi + ) if lateral_error_threshold < tmp_lateral_error or yaw_error_threshold < tmp_yaw_error: if Differential_Smoothing_Flag: @@ -1115,93 +620,141 @@ def timer_callback_traj(self): # [6] publish # [6-1] publish trajectory - pub_traj_len = min(int(50 / self.traj_step), aug_data_length) - tmp_traj = Trajectory() - for i in range(pub_traj_len): - tmp_traj_point = TrajectoryPoint() - tmp_traj_point.pose.position.x = trajectory_position_data[i + nearestIndex, 0] - tmp_traj_point.pose.position.y = trajectory_position_data[i + nearestIndex, 1] - tmp_traj_point.pose.position.z = present_position[2] - - tmp_traj_point.pose.orientation.x = 0.0 - tmp_traj_point.pose.orientation.y = 0.0 - tmp_traj_point.pose.orientation.z = np.sin( - trajectory_yaw_data[i + nearestIndex] / 2 - ) - tmp_traj_point.pose.orientation.w = np.cos( - trajectory_yaw_data[i + nearestIndex] / 2 + if self.course.closed: + pub_trajectory_index = np.arange( + self.nearestIndex, self.nearestIndex + int(50 / self.trajectory_step) + ) % len(trajectory_position_data) + else: + pub_trajectory_index = np.arange( + self.nearestIndex, + np.min( + [ + self.nearestIndex + int(50 / self.trajectory_step), + len(trajectory_position_data), + ] + ), ) - tmp_traj_point.longitudinal_velocity_mps = trajectory_longitudinal_velocity_data[ - i + nearestIndex - ] - tmp_traj.points.append(tmp_traj_point) + pub_trajectory_length = len(pub_trajectory_index) + tmp_trajectory = Trajectory() + for i in pub_trajectory_index: + tmp_trajectory_point = TrajectoryPoint() + tmp_trajectory_point.pose.position.x = trajectory_position_data[i, 0] + tmp_trajectory_point.pose.position.y = trajectory_position_data[i, 1] + tmp_trajectory_point.pose.position.z = present_position[2] + + tmp_trajectory_point.pose.orientation.x = 0.0 + tmp_trajectory_point.pose.orientation.y = 0.0 + tmp_trajectory_point.pose.orientation.z = np.sin(trajectory_yaw_data[i] / 2) + tmp_trajectory_point.pose.orientation.w = np.cos(trajectory_yaw_data[i] / 2) + + tmp_trajectory_point.longitudinal_velocity_mps = ( + trajectory_longitudinal_velocity_data[i] + ) + tmp_trajectory.points.append(tmp_trajectory_point) - self.trajectory_for_collecting_data_pub_.publish(tmp_traj) + self.trajectory_for_collecting_data_pub_.publish(tmp_trajectory) # [6-2] publish marker_array marker_array = MarkerArray() # [6-2a] local trajectory - marker_traj1 = Marker() - marker_traj1.type = 4 - marker_traj1.id = 1 - marker_traj1.header.frame_id = "map" + marker_trajectory_1 = Marker() + marker_trajectory_1.type = 4 + marker_trajectory_1.id = 1 + marker_trajectory_1.header.frame_id = "map" - marker_traj1.action = marker_traj1.ADD + marker_trajectory_1.action = marker_trajectory_1.ADD - marker_traj1.scale.x = 0.4 - marker_traj1.scale.y = 0.0 - marker_traj1.scale.z = 0.0 + marker_trajectory_1.scale.x = 0.4 + marker_trajectory_1.scale.y = 0.0 + marker_trajectory_1.scale.z = 0.0 - marker_traj1.color.a = 1.0 - marker_traj1.color.r = 1.0 - marker_traj1.color.g = 0.0 - marker_traj1.color.b = 0.0 + marker_trajectory_1.color.a = 1.0 + marker_trajectory_1.color.r = 1.0 + marker_trajectory_1.color.g = 0.0 + marker_trajectory_1.color.b = 0.0 - marker_traj1.lifetime.nanosec = 500000000 - marker_traj1.frame_locked = True + marker_trajectory_1.lifetime.nanosec = 500000000 + marker_trajectory_1.frame_locked = True - marker_traj1.points = [] - for i in range(len(tmp_traj.points)): + marker_trajectory_1.points = [] + for i in range(len(tmp_trajectory.points)): tmp_marker_point = Point() - tmp_marker_point.x = tmp_traj.points[i].pose.position.x - tmp_marker_point.y = tmp_traj.points[i].pose.position.y + tmp_marker_point.x = tmp_trajectory.points[i].pose.position.x + tmp_marker_point.y = tmp_trajectory.points[i].pose.position.y tmp_marker_point.z = 0.0 - marker_traj1.points.append(tmp_marker_point) + marker_trajectory_1.points.append(tmp_marker_point) + + marker_array.markers.append(marker_trajectory_1) + + # boundary + boundary_marker_trajectory_1 = Marker() + boundary_marker_trajectory_1.type = 4 + boundary_marker_trajectory_1.id = 3 + boundary_marker_trajectory_1.header.frame_id = "map" + + boundary_marker_trajectory_1.action = boundary_marker_trajectory_1.ADD + + boundary_marker_trajectory_1.scale.x = 0.4 + boundary_marker_trajectory_1.scale.y = 0.0 + boundary_marker_trajectory_1.scale.z = 0.0 + + boundary_marker_trajectory_1.color.a = 0.5 + boundary_marker_trajectory_1.color.r = 0.0 + boundary_marker_trajectory_1.color.g = 1.0 + boundary_marker_trajectory_1.color.b = 0.0 + + boundary_marker_trajectory_1.lifetime.nanosec = 500000000 + boundary_marker_trajectory_1.frame_locked = True + boundary_marker_trajectory_1.points = [] - marker_array.markers.append(marker_traj1) + down_sampling_ = 5 + for i in range(len(self.boundary_points) // down_sampling_): + boundary_tmp_marker_point = Point() + boundary_tmp_marker_point.x = self.boundary_points[i * down_sampling_][0] + boundary_tmp_marker_point.y = self.boundary_points[i * down_sampling_][1] + boundary_tmp_marker_point.z = 0.0 + boundary_marker_trajectory_1.points.append(boundary_tmp_marker_point) + + boundary_tmp_marker_point = Point() + boundary_tmp_marker_point.x = self.boundary_points[0][0] + boundary_tmp_marker_point.y = self.boundary_points[0][1] + boundary_tmp_marker_point.z = 0.0 + boundary_marker_trajectory_1.points.append(boundary_tmp_marker_point) + + marker_array.markers.append(boundary_marker_trajectory_1) # [6-2b] whole trajectory - marker_traj2 = Marker() - marker_traj2.type = 4 - marker_traj2.id = 0 - marker_traj2.header.frame_id = "map" + marker_trajectory_2 = Marker() + marker_trajectory_2.type = 4 + marker_trajectory_2.id = 0 + marker_trajectory_2.header.frame_id = "map" - marker_traj2.action = marker_traj2.ADD + marker_trajectory_2.action = marker_trajectory_2.ADD - marker_traj2.scale.x = 0.2 - marker_traj2.scale.y = 0.0 - marker_traj2.scale.z = 0.0 + marker_trajectory_2.scale.x = 0.2 + marker_trajectory_2.scale.y = 0.0 + marker_trajectory_2.scale.z = 0.0 - marker_traj2.color.a = 1.0 - marker_traj2.color.r = 0.0 - marker_traj2.color.g = 0.0 - marker_traj2.color.b = 1.0 + marker_trajectory_2.color.a = 1.0 + marker_trajectory_2.color.r = 0.0 + marker_trajectory_2.color.g = 0.0 + marker_trajectory_2.color.b = 1.0 - marker_traj2.lifetime.nanosec = 500000000 - marker_traj2.frame_locked = True + marker_trajectory_2.lifetime.nanosec = 500000000 + marker_trajectory_2.frame_locked = True - marker_traj2.points = [] - marker_downsampling = 5 - for i in range((len(trajectory_position_data) // marker_downsampling)): + marker_trajectory_2.points = [] + marker_down_sampling = 5 + for i in range(len(trajectory_position_data) // marker_down_sampling): tmp_marker_point = Point() - tmp_marker_point.x = trajectory_position_data[i * marker_downsampling, 0] - tmp_marker_point.y = trajectory_position_data[i * marker_downsampling, 1] + tmp_marker_point.x = trajectory_position_data[i * marker_down_sampling, 0] + tmp_marker_point.y = trajectory_position_data[i * marker_down_sampling, 1] tmp_marker_point.z = 0.0 - marker_traj2.points.append(tmp_marker_point) + marker_trajectory_2.points.append(tmp_marker_point) - marker_array.markers.append(marker_traj2) + marker_array.markers.append(marker_trajectory_2) # [6-2c] add arrow marker_arrow = Marker() @@ -1225,38 +778,52 @@ def timer_callback_traj(self): tangent_vec = np.array( [ - tmp_traj.points[1].pose.position.x - tmp_traj.points[0].pose.position.x, - tmp_traj.points[1].pose.position.y - tmp_traj.points[0].pose.position.y, + np.cos(trajectory_yaw_data[self.nearestIndex]), + np.sin(trajectory_yaw_data[self.nearestIndex]), ] ) marker_arrow.points = [] start_marker_point = Point() - start_marker_point.x = tmp_traj.points[0].pose.position.x - start_marker_point.y = tmp_traj.points[0].pose.position.y + start_marker_point.x = tmp_trajectory.points[0].pose.position.x + start_marker_point.y = tmp_trajectory.points[0].pose.position.y start_marker_point.z = 0.0 marker_arrow.points.append(start_marker_point) end_marker_point = Point() - end_marker_point.x = tmp_traj.points[0].pose.position.x + 5.0 * tangent_vec[ - 0 - ] / np.linalg.norm(tangent_vec) - end_marker_point.y = tmp_traj.points[0].pose.position.y + 5.0 * tangent_vec[ - 1 - ] / np.linalg.norm(tangent_vec) + end_marker_point.x = tmp_trajectory.points[0].pose.position.x + 5.0 * tangent_vec[0] + end_marker_point.y = tmp_trajectory.points[0].pose.position.y + 5.0 * tangent_vec[1] end_marker_point.z = 0.0 marker_arrow.points.append(end_marker_point) marker_array.markers.append(marker_arrow) self.data_collecting_trajectory_marker_array_pub_.publish(marker_array) + # [6-3] stop request - if not self.checkInDateCollectingArea(present_position): + # self.get_logger().info("self.course.check_in_boundary(present_position): {}".format(self.course.check_in_boundary(present_position))) + if not self.course.check_in_boundary(present_position): msg = Bool() msg.data = True self.pub_stop_request_.publish(msg) + # [6-4] update trajectory data if necessary + ( + self.nearestIndex, + self.trajectory_position_data, + self.trajectory_yaw_data, + self.trajectory_curvature_data, + self.trajectory_parts, + self.trajectory_achievement_rates, + ) = self.course.update_trajectory_points( + self.nearestIndex, + self.yaw_offset, + self.rectangle_center_position, + self.collected_data_counts_of_vel_acc, + self.collected_data_counts_of_vel_steer, + ) + if debug_matplotlib_plot_flag: self.axs[0].cla() step_size_array = np.sqrt( @@ -1267,41 +834,43 @@ def timer_callback_traj(self): distance = np.zeros(len(trajectory_position_data)) for i in range(1, len(trajectory_position_data)): distance[i] = distance[i - 1] + step_size_array[i - 1] - distance -= distance[nearestIndex] + distance -= distance[self.nearestIndex] time_width_array = step_size_array / ( trajectory_longitudinal_velocity_data[:-1] + 0.01 ) timestamp = np.zeros(len(trajectory_position_data)) for i in range(1, len(trajectory_position_data)): timestamp[i] = timestamp[i - 1] + time_width_array[i - 1] - timestamp -= timestamp[nearestIndex] + timestamp -= timestamp[self.nearestIndex] self.axs[0].plot(0, present_linear_velocity[0], "o", label="current vel") self.axs[0].plot( - timestamp[nearestIndex : nearestIndex + pub_traj_len], + timestamp[self.nearestIndex : self.nearestIndex + pub_trajectory_length], trajectory_longitudinal_velocity_data_without_limit[ - nearestIndex : nearestIndex + pub_traj_len + self.nearestIndex : self.nearestIndex + pub_trajectory_length ], "--", label="target vel before applying limit", ) self.axs[0].plot( - timestamp[nearestIndex : nearestIndex + pub_traj_len], - lateral_acc_limit[nearestIndex : nearestIndex + pub_traj_len], + timestamp[self.nearestIndex : self.nearestIndex + pub_trajectory_length], + lateral_acc_limit[ + self.nearestIndex : self.nearestIndex + pub_trajectory_length + ], "--", label="lateral acc limit (always)", ) self.axs[0].plot( - timestamp[nearestIndex : nearestIndex + pub_traj_len], - velocity_limit_by_tracking_error * np.ones(pub_traj_len), + timestamp[self.nearestIndex : self.nearestIndex + pub_trajectory_length], + velocity_limit_by_tracking_error * np.ones(pub_trajectory_length), "--", label="vel limit by tracking error (only when exceeding threshold)", ) self.axs[0].plot( - timestamp[nearestIndex : nearestIndex + pub_traj_len], + timestamp[self.nearestIndex : self.nearestIndex + pub_trajectory_length], trajectory_longitudinal_velocity_data[ - nearestIndex : nearestIndex + pub_traj_len + self.nearestIndex : self.nearestIndex + pub_trajectory_length ], label="actual target vel", ) diff --git a/control_data_collecting_tool/scripts/params.py b/control_data_collecting_tool/scripts/params.py new file mode 100644 index 00000000..16c6224e --- /dev/null +++ b/control_data_collecting_tool/scripts/params.py @@ -0,0 +1,69 @@ +#!/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 + + +class Params: + def __init__(self, param_dict): + self.acc_kp = param_dict["acc_kp"] + self.max_lateral_accel = param_dict["max_lateral_accel"] + + """ + velocity and acceleration grid + velocity and steer grid + """ + self.num_bins_v = param_dict["NUM_BINS_V"] + self.num_bins_steer = param_dict["NUM_BINS_STEER"] + self.num_bins_a = param_dict["NUM_BINS_A"] + self.v_min, self.v_max = (param_dict["V_MIN"], param_dict["V_MAX"]) + self.steer_min, self.steer_max = (param_dict["STEER_MIN"], param_dict["STEER_MAX"]) + self.a_min, self.a_max = (param_dict["A_MIN"], param_dict["A_MAX"]) + + self.collected_data_counts_of_vel_acc = np.zeros( + (self.num_bins_v, self.num_bins_a), dtype=np.int32 + ) + self.collected_data_counts_of_vel_steer = np.zeros( + (self.num_bins_v, self.num_bins_steer), dtype=np.int32 + ) + + self.v_bins = np.linspace(self.v_min, self.v_max, self.num_bins_v + 1) + self.steer_bins = np.linspace(self.steer_min, self.steer_max, self.num_bins_steer + 1) + self.a_bins = np.linspace(self.a_min, self.a_max, self.num_bins_a + 1) + + self.v_bin_centers = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + collecting_data_min_v, collecting_data_max_v = ( + param_dict["COLLECTING_DATA_V_MIN"], + param_dict["COLLECTING_DATA_V_MAX"], + ) + + collecting_data_min_a, collecting_data_max_a = ( + param_dict["COLLECTING_DATA_A_MIN"], + param_dict["COLLECTING_DATA_A_MAX"], + ) + + self.collecting_data_min_n_v = max([np.digitize(collecting_data_min_v, self.v_bins) - 1, 0]) + self.collecting_data_max_n_v = ( + min([np.digitize(collecting_data_max_v, self.v_bins) - 1, self.num_bins_v - 1]) + 1 + ) + + self.collecting_data_min_n_a = max([np.digitize(collecting_data_min_a, self.v_bins) - 1, 0]) + self.collecting_data_max_n_a = ( + min([np.digitize(collecting_data_max_a, self.v_bins) - 1, self.num_bins_a - 1]) + 1 + ) diff --git a/control_data_collecting_tool/src/data_collecting_goal_pose.cpp b/control_data_collecting_tool/src/data_collecting_goal_pose.cpp new file mode 100644 index 00000000..4705f477 --- /dev/null +++ b/control_data_collecting_tool/src/data_collecting_goal_pose.cpp @@ -0,0 +1,133 @@ +// Copyright 2024 Proxima Technology Inc, TIER IV Inc. +// +// 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. + +#include "data_collecting_goal_pose.hpp" + +#include // Include for ROS logging +#include // Include for access to DisplayContext + +#include +#include + +namespace rviz_plugins +{ +// Constructor: Initializes custom QoS profile +DataCollectingGoalPose::DataCollectingGoalPose() : custom_qos_profile_(rclcpp::QoS(5)) +{ + RCLCPP_INFO(rclcpp::get_logger("DataCollectingGoalPose"), "DataCollectingGoalPose initialized"); +} + +// Callback function for receiving OperationModeState messages +void DataCollectingGoalPose::onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("DataCollectingGoalPose"), + "Received OperationModeState: mode=%d, is_autoware_control_enabled=%d", msg->mode, + msg->is_autoware_control_enabled); + + // Update control_applying_ flag based on mode and control status + if (msg->mode == 3 && msg->is_autoware_control_enabled) { + control_applying_ = true; + RCLCPP_INFO(rclcpp::get_logger("DataCollectingGoalPose"), "Control is applying"); + } else { + control_applying_ = false; + RCLCPP_INFO(rclcpp::get_logger("DataCollectingGoalPose"), "Control is NOT applying"); + } +} + +// Destructor +DataCollectingGoalPose::~DataCollectingGoalPose() +{ + RCLCPP_INFO(rclcpp::get_logger("DataCollectingGoalPose"), "DataCollectingGoalPose destroyed"); +} + +// Initialization function for the DataCollectingGoalPose tool in RViz +void DataCollectingGoalPose::onInitialize() +{ + RCLCPP_INFO( + rclcpp::get_logger("DataCollectingGoalPose"), "Initializing DataCollectingGoalPose tool"); + + // Call the base class initialization + GoalTool::onInitialize(); + setName("DataCollectingGoalPose"); + + // Initialize ROS 2 node and create the PoseStamped publisher with custom QoS + auto raw_node = context_->getRosNodeAbstraction().lock(); + if (!raw_node) { + RCLCPP_ERROR( + rclcpp::get_logger("DataCollectingGoalPose"), "Failed to lock ROS node abstraction"); + return; + } + + auto raw_node_ptr = raw_node->get_raw_node(); + pose_publisher_ = raw_node_ptr->create_publisher( + "/data_collecting_goal_pose", custom_qos_profile_); + RCLCPP_INFO( + rclcpp::get_logger("DataCollectingGoalPose"), + "Pose publisher initialized on topic: /data_collecting_goal_pose"); + + // Subscription to the operation mode state topic with a QoS of 1 + sub_operation_mode_state_ = + raw_node_ptr->create_subscription( + "/system/operation_mode/state", rclcpp::QoS{1}, + std::bind(&DataCollectingGoalPose::onOperationModeState, this, std::placeholders::_1)); + RCLCPP_INFO( + rclcpp::get_logger("DataCollectingGoalPose"), + "Subscribed to topic: /system/operation_mode/state"); +} + +// Override onPoseSet to set pose only when control is not applying +void DataCollectingGoalPose::onPoseSet(double x, double y, double theta) +{ + RCLCPP_INFO( + rclcpp::get_logger("DataCollectingGoalPose"), + "onPoseSet called with x=%.2f, y=%.2f, theta=%.2f", x, y, theta); + + if (control_applying_) { + RCLCPP_WARN( + rclcpp::get_logger("DataCollectingGoalPose"), + "Pose setting ignored because control is applying"); + return; + } + + // Setup Message to publish + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = rclcpp::Clock().now(); + pose_msg.header.frame_id = "map"; + pose_msg.pose.position.x = x; + pose_msg.pose.position.y = y; + pose_msg.pose.position.z = 0.0; + pose_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, sin(theta / 2), cos(theta / 2))); + + RCLCPP_INFO( + rclcpp::get_logger("DataCollectingGoalPose"), "Publishing pose: x=%.2f, y=%.2f, theta=%.2f", x, + y, theta); + + try { + pose_publisher_->publish(pose_msg); + RCLCPP_INFO( + rclcpp::get_logger("DataCollectingGoalPose"), + "Pose successfully published to /data_collecting_goal_pose"); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("DataCollectingGoalPose"), "Failed to publish pose: %s", e.what()); + } +} + +} // namespace rviz_plugins + +#include +// Export the plugin class so it can be dynamically loaded by RViz +PLUGINLIB_EXPORT_CLASS(rviz_plugins::DataCollectingGoalPose, rviz_common::Tool) diff --git a/control_data_collecting_tool/src/data_collecting_goal_pose.hpp b/control_data_collecting_tool/src/data_collecting_goal_pose.hpp new file mode 100644 index 00000000..e107df5e --- /dev/null +++ b/control_data_collecting_tool/src/data_collecting_goal_pose.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 Proxima Technology Inc, TIER IV Inc. +// +// 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. + +#ifndef DATA_COLLECTING_GOAL_POSE_HPP_ +#define DATA_COLLECTING_GOAL_POSE_HPP_ + +#include "rviz_common/properties/qos_profile_property.hpp" +#include "rviz_common/properties/string_property.hpp" + +#include +#include + +#include +#include + +namespace rviz_plugins +{ +// DataCollectingGoalPose class that extends GoalTool to add data collection features +class DataCollectingGoalPose : public rviz_default_plugins::tools::GoalTool +{ + Q_OBJECT +public: + // Constructor + DataCollectingGoalPose(); + // Destructor + ~DataCollectingGoalPose(); + + // Called during tool initialization in RViz + void onInitialize() override; + + // Sets the pose using specified coordinates and orientation + void onPoseSet(double x, double y, double theta) override; + +private: + // Flag to indicate if control is being applied + bool control_applying_ = false; + + // Subscription to listen for operation mode state messages + rclcpp::Subscription::SharedPtr + sub_operation_mode_state_; + + // Callback function to handle received operation mode state messages + void onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + + // Custom QoS profile for message publishing + rclcpp::QoS custom_qos_profile_; + // Shared pointer to the raw ROS 2 node used for publishing + rclcpp::Node::SharedPtr raw_node_; + // Publisher to send PoseStamped messages to a specific topic + rclcpp::Publisher::SharedPtr pose_publisher_; +}; +} // namespace rviz_plugins + +#endif // DATA_COLLECTING_GOAL_POSE_HPP_