diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index 6d817fcd..e224634c 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -8,6 +8,10 @@ install(PROGRAMS scripts/data_collecting_pure_pursuit_trajectory_follower.py scripts/data_collecting_trajectory_publisher.py scripts/data_collecting_plotter.py + scripts/data_collecting_rosbag_record.py + scripts/data_collecting_data_counter.py + scripts/data_collecting_base_node.py + scripts/rosbag_play.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index 4f853eec..db8ca8d3 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -33,6 +33,12 @@ This package provides tools for automatically collecting data using pure pursuit ros2 launch control_data_collecting_tool control_data_collecting_tool.launch.py ``` + - 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. + + - 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.) + 5. Add visualization in rviz: - `/data_collecting_area` @@ -53,13 +59,7 @@ This package provides tools for automatically collecting data using pure pursuit > [!NOTE] > You cannot change the data collecting area while driving. -7. start recording rosbag data. For example, run the following command: - - ```bash - ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /sensing/imu/imu_data /system/operation_mode/state /vehicle/status/control_mode /external/selected/control_cmd /external/selected/gear_cmd /data_collecting_trajectory - ``` - -8. Click the `LOCAL` button on `OperationMode` in `AutowareStatePanel`. +7. Click the `LOCAL` button on `OperationMode` in `AutowareStatePanel`. @@ -67,17 +67,20 @@ This package provides tools for automatically collecting data using pure pursuit -9. If you want to stop data collecting automatic driving, run the following command +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 ``` -10. If you want to restart data collecting automatic driving, run the following command + > [!NOTE] + > When the car crosses the green boundary line, a similar stopping procedure will be automatically triggered. -```bash -ros2 topic pub /data_collecting_stop_request std_msgs/msg/Bool "data: false" --once -``` +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 + ``` ## Change Courses @@ -95,22 +98,25 @@ You can change the course by selecting `COURSE_NAME` in `config/param.yaml` from ## Parameter -ROS 2 params in `/data_collecting_trajectory_publisher` node: +ROS 2 params which are common in all nodes: | 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_ACCELERATION` | `int` | Number of bins of acceleration 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 | -| `wheel_base` | `double` | Wheel base [m] | 2.79 | -| `acc_kp` | `double` | Accel command proportional gain | 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 | @@ -120,43 +126,22 @@ ROS 2 params in `/data_collecting_trajectory_publisher` node: | `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 | - -ROS 2 params in `/data_collecting_pure_pursuit_trajectory_follower` node: - -| Name | Type | Description | Default value | -| :--------------------------------------- | :------- | :------------------------------------------------------------- | :------------ | -| `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] | 1.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 | - -ROS 2 params in `/data_collecting_plotter` node: - -| Name | Type | Description | Default value | -| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- | -| `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_ACCELERATION` | `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 | -| `wheel_base` | `double` | Wheel base [m] | 2.79 | +| `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 | diff --git a/control_data_collecting_tool/config/param.yaml b/control_data_collecting_tool/config/param.yaml index d96553ac..e14b66a8 100644 --- a/control_data_collecting_tool/config/param.yaml +++ b/control_data_collecting_tool/config/param.yaml @@ -1,19 +1,7 @@ -data_collecting_plotter: +/**: ros__parameters: - 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 - - wheel_base: 2.79 + LOAD_ROSBAG2_FILES: true -data_collecting_trajectory_publisher: - ros__parameters: COURSE_NAME: eight_course # COURSE_NAME: u_shaped_return # COURSE_NAME: straight_line_positive @@ -29,8 +17,11 @@ data_collecting_trajectory_publisher: A_MIN: -1.0 A_MAX: 1.0 - wheel_base: 2.79 - acc_kp: 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 @@ -41,9 +32,8 @@ data_collecting_trajectory_publisher: longitudinal_velocity_noise_min_period: 5.0 longitudinal_velocity_noise_max_period: 20.0 -data_collecting_pure_pursuit_trajectory_follower: - ros__parameters: pure_pursuit_type: linearized + # pure_pursuit_type: naive wheel_base: 2.79 acc_kp: 1.0 lookahead_time: 2.0 @@ -51,7 +41,7 @@ data_collecting_pure_pursuit_trajectory_follower: linearized_pure_pursuit_steer_kp_param: 2.0 linearized_pure_pursuit_steer_kd_param: 2.0 stop_acc: -2.0 - stop_jerk_lim: 2.0 + stop_jerk_lim: 5.0 lon_acc_lim: 2.0 lon_jerk_lim: 5.0 steer_lim: 1.0 diff --git a/control_data_collecting_tool/config/topics.yaml b/control_data_collecting_tool/config/topics.yaml new file mode 100644 index 00000000..6fc3ec9a --- /dev/null +++ b/control_data_collecting_tool/config/topics.yaml @@ -0,0 +1,10 @@ +topics: + - /localization/kinematic_state + - /localization/acceleration + - /vehicle/status/steering_status + - /sensing/imu/imu_data + - /system/operation_mode/state + - /vehicle/status/control_mode + - /external/selected/control_cmd + - /external/selected/gear_cmd + - /data_collecting_trajectory 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 ba3ea314..475c7023 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 @@ -45,6 +45,17 @@ def generate_launch_description(): name="data_collecting_plotter", parameters=[param_file_path], ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_rosbag_record.py", + name="data_collecting_rosbag_record", + ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_data_counter.py", + name="data_collecting_data_counter", + parameters=[param_file_path], + ), ] ) diff --git a/control_data_collecting_tool/scripts/data_collecting_base_node.py b/control_data_collecting_tool/scripts/data_collecting_base_node.py new file mode 100644 index 00000000..885e6d7a --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_base_node.py @@ -0,0 +1,171 @@ +#!/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 autoware_adapi_v1_msgs.msg import OperationModeState +from autoware_vehicle_msgs.msg import ControlModeReport +from geometry_msgs.msg import AccelWithCovarianceStamped +from nav_msgs.msg import Odometry +import numpy as np +from rcl_interfaces.msg import ParameterDescriptor +from rclpy.node import Node + + +class DataCollectingBaseNode(Node): + def __init__(self, node_name): + super().__init__(node_name) + + # common params + self.declare_parameter( + "NUM_BINS_V", + 10, + ParameterDescriptor(description="Number of bins of velocity in heatmap"), + ) + + self.declare_parameter( + "NUM_BINS_STEER", + 10, + ParameterDescriptor(description="Number of bins of steer in heatmap"), + ) + + self.declare_parameter( + "NUM_BINS_A", + 10, + ParameterDescriptor(description="Number of bins of acceleration in heatmap"), + ) + + self.declare_parameter( + "V_MIN", + 0.0, + ParameterDescriptor(description="Minimum velocity in heatmap [m/s]"), + ) + + self.declare_parameter( + "V_MAX", + 11.5, + ParameterDescriptor(description="Maximum velocity in heatmap [m/s]"), + ) + + self.declare_parameter( + "STEER_MIN", + -1.0, + ParameterDescriptor(description="Minimum steer in heatmap [rad]"), + ) + + self.declare_parameter( + "STEER_MAX", + 1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) + + self.declare_parameter( + "A_MIN", + -1.0, + ParameterDescriptor(description="Minimum acceleration in heatmap [m/ss]"), + ) + + self.declare_parameter( + "A_MAX", + 1.0, + ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), + ) + + self.declare_parameter( + "wheel_base", + 2.79, + ParameterDescriptor(description="Wheel base [m]"), + ) + + self.sub_odometry_ = self.create_subscription( + Odometry, + "/localization/kinematic_state", + self.onOdometry, + 1, + ) + + self.sub_acceleration_ = self.create_subscription( + AccelWithCovarianceStamped, + "/localization/acceleration", + self.onAcceleration, + 1, + ) + + self.operation_mode_subscription_ = self.create_subscription( + OperationModeState, + "/system/operation_mode/state", + self.subscribe_operation_mode, + 10, + ) + + self.control_mode_subscription_ = self.create_subscription( + ControlModeReport, + "/vehicle/status/control_mode", + self.subscribe_control_mode, + 10, + ) + + self._present_kinematic_state = None + self._present_acceleration = None + self.present_operation_mode_ = None + self._present_control_mode_ = None + + """ + velocity and acceleration grid + velocity and steer grid + """ + self.num_bins_v = self.get_parameter("NUM_BINS_V").get_parameter_value().integer_value + self.num_bins_steer = ( + self.get_parameter("NUM_BINS_STEER").get_parameter_value().integer_value + ) + self.num_bins_a = self.get_parameter("NUM_BINS_A").get_parameter_value().integer_value + self.v_min, self.v_max = ( + self.get_parameter("V_MIN").get_parameter_value().double_value, + self.get_parameter("V_MAX").get_parameter_value().double_value, + ) + self.steer_min, self.steer_max = ( + self.get_parameter("STEER_MIN").get_parameter_value().double_value, + self.get_parameter("STEER_MAX").get_parameter_value().double_value, + ) + self.a_min, self.a_max = ( + self.get_parameter("A_MIN").get_parameter_value().double_value, + self.get_parameter("A_MAX").get_parameter_value().double_value, + ) + + 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 + + def onOdometry(self, msg): + self._present_kinematic_state = msg + + def onAcceleration(self, msg): + self._present_acceleration = msg + + def subscribe_operation_mode(self, msg): + self.present_operation_mode_ = msg.mode + + def subscribe_control_mode(self, msg): + self._present_control_mode_ = msg.mode diff --git a/control_data_collecting_tool/scripts/data_collecting_data_counter.py b/control_data_collecting_tool/scripts/data_collecting_data_counter.py new file mode 100755 index 00000000..6c441546 --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_data_counter.py @@ -0,0 +1,279 @@ +#!/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 os + +from data_collecting_base_node import DataCollectingBaseNode +import numpy as np +from numpy import arctan2 +from rcl_interfaces.msg import ParameterDescriptor +import rclpy +import rosbag_play +from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Int32MultiArray +from std_msgs.msg import MultiArrayDimension +from std_msgs.msg import MultiArrayLayout + + +def publish_Int32MultiArray(publisher_, array_data): + msg = Int32MultiArray() + + flattened_data = array_data.flatten().tolist() + msg.data = flattened_data + + layout = MultiArrayLayout() + + dim1 = MultiArrayDimension() + dim1.label = "rows" + dim1.size = len(array_data) + dim1.stride = len(flattened_data) + + dim2 = MultiArrayDimension() + dim2.label = "cols" + dim2.size = len(array_data[0]) + dim2.stride = len(array_data[0]) + + layout.dim.append(dim1) + layout.dim.append(dim2) + + msg.layout = layout + + publisher_.publish(msg) + + +# inherits from DataCollectingBaseNode +class DataCollectingDataCounter(DataCollectingBaseNode): + def __init__(self): + super().__init__("data_collecting_data_counter") + + self.vel_hist = deque([float(0.0)] * 200, maxlen=200) + self.acc_hist = deque([float(0.0)] * 200, maxlen=200) + + self.timer_period_callback = 0.033 # 30ms + self.timer_counter = self.create_timer( + self.timer_period_callback, + self.timer_callback_counter, + ) + + self.collected_data_counts_of_vel_acc_publisher_ = self.create_publisher( + Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_acc", 10 + ) + self.collected_data_counts_of_vel_steer_publisher_ = self.create_publisher( + Int32MultiArray, "/control_data_collecting_tools/collected_data_counts_of_vel_steer", 10 + ) + + self.vel_hist_publisher_ = self.create_publisher( + Float32MultiArray, "/control_data_collecting_tools/vel_hist", 10 + ) + self.acc_hist_publisher_ = self.create_publisher( + Float32MultiArray, "/control_data_collecting_tools/acc_hist", 10 + ) + + self.declare_parameter( + "LOAD_ROSBAG2_FILES", + True, + ParameterDescriptor( + description="Flag that determines whether to load rosbag2 data or not" + ), + ) + + load_rosbag2_files = ( + self.get_parameter("LOAD_ROSBAG2_FILES").get_parameter_value().bool_value + ) + + if load_rosbag2_files: + # candidates referencing the rosbag data + rosbag2_dir_list = [d for d in os.listdir("./") if os.path.isdir(os.path.join("./", d))] + # load rosbag data + self.load_rosbag_data(rosbag2_dir_list) + + def load_rosbag_data(self, rosbag2_dir_list): + for rosbag2_dir in rosbag2_dir_list: + # try to fetch /localization/acceleration and /localization/kinematic_state from rosbag2_file + rosbag2_file = "./" + rosbag2_dir + "/" + rosbag2_dir + "_0.db3" + db3reader = rosbag_play.db3Reader(rosbag2_file) + + load_acc_topic = db3reader.load_db3("/localization/acceleration") + load_kinematic_topic = db3reader.load_db3("/localization/kinematic_state") + + # if /localization/acceleration or /localization/kinematic_state is not include in the data base skip counting data points + if load_acc_topic and load_kinematic_topic: + acceleration, kinematic_state = db3reader.read_msg( + "/localization/acceleration" + ), db3reader.read_msg("/localization/kinematic_state") + if acceleration is None or kinematic_state is None: + continue + + previous_acc, previous_vel, previous_ang_vel_z = ( + acceleration.accel.accel.linear.x, + kinematic_state.twist.twist.linear.x, + kinematic_state.twist.twist.angular.z, + ) + previous_acc_time, previous_kinematic_time = ( + acceleration.header.stamp.sec + 1e-9 * acceleration.header.stamp.nanosec, + kinematic_state.header.stamp.sec + 1e-9 * kinematic_state.header.stamp.nanosec, + ) + current_time = max([previous_acc_time, previous_kinematic_time]) + + acceleration, kinematic_state = db3reader.read_msg( + "/localization/acceleration" + ), db3reader.read_msg("/localization/kinematic_state") + if acceleration is None or kinematic_state is None: + break + current_acc_time, current_kinematic_time = ( + acceleration.header.stamp.sec + 1e-9 * acceleration.header.stamp.nanosec, + kinematic_state.header.stamp.sec + 1e-9 * kinematic_state.header.stamp.nanosec, + ) + + # A while loop for counting data points + while True: + # interpolate acceleration if necessary + while current_time > current_acc_time: + previous_acc = acceleration.accel.accel.linear.x + acceleration = db3reader.read_msg("/localization/acceleration") + if acceleration is None: + break + previous_acc_time = current_acc_time + current_acc_time = ( + acceleration.header.stamp.sec + 1e-9 * acceleration.header.stamp.nanosec + ) + + if acceleration is None: + break + acc = (current_acc_time - current_time) * acceleration.accel.accel.linear.x + ( + current_time - previous_acc_time + ) * previous_acc + acc /= current_acc_time - previous_acc_time + + # interpolate kinematic state if necessary + while current_time > current_kinematic_time: + previous_vel, previous_ang_vel_z = ( + kinematic_state.twist.twist.linear.x, + kinematic_state.twist.twist.angular.z, + ) + kinematic_state = db3reader.read_msg("/localization/kinematic_state") + if kinematic_state is None: + break + previous_kinematic_time = current_kinematic_time + current_kinematic_time = ( + kinematic_state.header.stamp.sec + + 1e-9 * kinematic_state.header.stamp.nanosec + ) + + if kinematic_state is None: + break + vel = ( + current_kinematic_time - current_time + ) * kinematic_state.twist.twist.linear.x + ( + current_time - previous_kinematic_time + ) * previous_vel + vel /= current_kinematic_time - previous_kinematic_time + + ang_vel_z = ( + current_kinematic_time - current_time + ) * kinematic_state.twist.twist.angular.z + ( + current_time - previous_kinematic_time + ) * previous_ang_vel_z + ang_vel_z /= current_kinematic_time - previous_kinematic_time + + # calculation of steer + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + steer = arctan2(wheel_base * ang_vel_z, kinematic_state.twist.twist.linear.x) + + # count number of data + if kinematic_state.twist.twist.linear.x > 1e-3: + self.count_observations( + vel, + acc, + steer, + ) + + current_time += self.timer_period_callback + + def count_observations(self, v, a, steer): + v_bin = np.digitize(v, self.v_bins) - 1 + steer_bin = np.digitize(steer, self.steer_bins) - 1 + a_bin = np.digitize(a, self.a_bins) - 1 + + if 0 <= v_bin < self.num_bins_v and 0 <= a_bin < self.num_bins_a: + self.collected_data_counts_of_vel_acc[v_bin, a_bin] += 1 + + if 0 <= v_bin < self.num_bins_v and 0 <= steer_bin < self.num_bins_steer: + self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 + + # call back for counting data points + def timer_callback_counter(self): + if ( + self._present_kinematic_state is not None + and self._present_acceleration is not None + and self.present_operation_mode_ == 3 + and self._present_control_mode_ == 1 + ): + # calculate steer + angular_z = self._present_kinematic_state.twist.twist.angular.z + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + current_steer = arctan2( + wheel_base * angular_z, self._present_kinematic_state.twist.twist.linear.x + ) + + current_vel = self._present_kinematic_state.twist.twist.linear.x + current_acc = self._present_acceleration.accel.accel.linear.x + + if self._present_kinematic_state.twist.twist.linear.x > 1e-3: + self.count_observations( + current_vel, + current_acc, + current_steer, + ) + + self.acc_hist.append(float(current_acc)) + self.vel_hist.append(float(current_vel)) + + # publish collected_data_counts_of_vel_acc + publish_Int32MultiArray( + self.collected_data_counts_of_vel_acc_publisher_, self.collected_data_counts_of_vel_acc + ) + + # publish collected_data_counts_of_vel_steer + publish_Int32MultiArray( + self.collected_data_counts_of_vel_steer_publisher_, + self.collected_data_counts_of_vel_steer, + ) + + # publish acc_hist + msg = Float32MultiArray() + msg.data = list(self.acc_hist) + self.acc_hist_publisher_.publish(msg) + + # publish vel_hist + msg = Float32MultiArray() + msg.data = list(self.vel_hist) + self.vel_hist_publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + data_collecting_data_counter = DataCollectingDataCounter() + rclpy.spin(data_collecting_data_counter) + + data_collecting_data_counter.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control_data_collecting_tool/scripts/data_collecting_plotter.py b/control_data_collecting_tool/scripts/data_collecting_plotter.py index 30f1f2cc..93c35276 100755 --- a/control_data_collecting_tool/scripts/data_collecting_plotter.py +++ b/control_data_collecting_tool/scripts/data_collecting_plotter.py @@ -14,238 +14,88 @@ # See the License for the specific language governing permissions and # limitations under the License. -import threading - -from geometry_msgs.msg import AccelWithCovarianceStamped +from data_collecting_base_node import DataCollectingBaseNode import matplotlib.pyplot as plt -from nav_msgs.msg import Odometry import numpy as np -from numpy import arctan2 -from rcl_interfaces.msg import ParameterDescriptor import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node import seaborn as sns +from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Int32MultiArray -class DataCollectingPlotter(Node): +class DataCollectingPlotter(DataCollectingBaseNode): def __init__(self): super().__init__("data_collecting_plotter") - self.declare_parameter( - "NUM_BINS_V", - 10, - ParameterDescriptor(description="Number of bins of velocity in heatmap"), - ) - - self.declare_parameter( - "NUM_BINS_STEER", - 10, - ParameterDescriptor(description="Number of bins of steer in heatmap"), - ) - - self.declare_parameter( - "NUM_BINS_A", - 10, - ParameterDescriptor(description="Number of bins of acceleration in heatmap"), - ) - - self.declare_parameter( - "V_MIN", - 0.0, - ParameterDescriptor(description="Maximum velocity in heatmap [m/s]"), - ) - - self.declare_parameter( - "V_MAX", - 1.0, - ParameterDescriptor(description="Minimum steer in heatmap [m/s]"), - ) - - self.declare_parameter( - "STEER_MIN", - -1.0, - ParameterDescriptor(description="Maximum steer in heatmap [rad]"), - ) - - self.declare_parameter( - "STEER_MAX", - 1.0, - ParameterDescriptor(description="Maximum steer in heatmap [rad]"), - ) - - self.declare_parameter( - "A_MIN", - -1.0, - ParameterDescriptor(description="Minimum acceleration in heatmap [m/ss]"), - ) - - self.declare_parameter( - "A_MAX", - 1.0, - ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), - ) - - self.declare_parameter( - "wheel_base", - 2.79, # sample_vehicle_launch/sample_vehicle_description/config/vehicle_info.param.yaml - ParameterDescriptor(description="Wheel base [m]"), - ) - - self.sub_odometry_ = self.create_subscription( - Odometry, - "/localization/kinematic_state", - self.onOdometry, - 1, - ) - - self.sub_acceleration_ = self.create_subscription( - AccelWithCovarianceStamped, - "/localization/acceleration", - self.onAcceleration, - 1, - ) - - self._present_kinematic_state = None - self._present_acceleration = None - - # velocity and acceleration grid - # velocity and steer grid - self.num_bins_v = self.get_parameter("NUM_BINS_V").get_parameter_value().integer_value - self.num_bins_steer = ( - self.get_parameter("NUM_BINS_STEER").get_parameter_value().integer_value - ) - self.num_bins_a = self.get_parameter("NUM_BINS_A").get_parameter_value().integer_value - self.v_min, self.v_max = ( - self.get_parameter("V_MIN").get_parameter_value().double_value, - self.get_parameter("V_MAX").get_parameter_value().double_value, - ) - self.steer_min, self.steer_max = ( - self.get_parameter("STEER_MIN").get_parameter_value().double_value, - self.get_parameter("STEER_MAX").get_parameter_value().double_value, - ) - self.a_min, self.a_max = ( - self.get_parameter("A_MIN").get_parameter_value().double_value, - self.get_parameter("A_MAX").get_parameter_value().double_value, - ) - - self.collected_data_counts_of_vel_acc = np.zeros((self.num_bins_v, self.num_bins_a)) - self.collected_data_counts_of_vel_steer = np.zeros((self.num_bins_v, self.num_bins_steer)) - - 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 - - self.vel_hist = np.zeros(200) - self.acc_hist = np.zeros(200) - - self.vel_hist_for_plot = np.zeros(200) - self.acc_hist_for_plot = np.zeros(200) - - # create callback groups - self.callback_group = ReentrantCallbackGroup() - self.lock = threading.Lock() - - # callback for data count - self.timer_period_callback = 0.033 # 30ms - self.timer_counter = self.create_timer( - self.timer_period_callback, - self.timer_callback_counter, - callback_group=self.callback_group, - ) - # callback for plot self.grid_update_time_interval = 5.0 self.timer_plotter = self.create_timer( self.grid_update_time_interval, self.timer_callback_plotter, - callback_group=self.callback_group, ) self.fig, self.axs = plt.subplots(3, 1, figsize=(12, 20)) plt.ion() - self.collected_data_counts_of_vel_acc_for_plot = np.zeros( - (self.num_bins_v, self.num_bins_a) - ) - self.collected_data_counts_of_vel_steer_for_plot = np.zeros( - (self.num_bins_v, self.num_bins_steer) + self.collected_data_counts_of_vel_acc_subscription_ = self.create_subscription( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_acc", + self.subscribe_collected_data_counts_of_vel_acc, + 10, ) + self.collected_data_counts_of_vel_acc_subscription_ - self.v_bin_centers_for_plot = (self.v_bins[:-1] + self.v_bins[1:]) / 2 - self.steer_bin_centers_for_plot = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 - self.a_bin_centers_for_plot = (self.a_bins[:-1] + self.a_bins[1:]) / 2 - - def onOdometry(self, msg): - self._present_kinematic_state = msg + self.collected_data_counts_of_vel_steer_subscription_ = self.create_subscription( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_steer", + self.subscribe_collected_data_counts_of_vel_steer, + 10, + ) + self.collected_data_counts_of_vel_steer_subscription_ - def onAcceleration(self, msg): - self._present_acceleration = msg + self.acc_hist_subscription_ = self.create_subscription( + Float32MultiArray, + "/control_data_collecting_tools/acc_hist", + self.subscribe_acc_hist, + 10, + ) + self.acc_hist_subscription_ - def count_observations(self, v, a, steer): - v_bin = np.digitize(v, self.v_bins) - 1 - steer_bin = np.digitize(steer, self.steer_bins) - 1 - a_bin = np.digitize(a, self.a_bins) - 1 + self.vel_hist_subscription_ = self.create_subscription( + Float32MultiArray, + "/control_data_collecting_tools/vel_hist", + self.subscribe_vel_hist, + 10, + ) + self.vel_hist_subscription_ - if 0 <= v_bin < self.num_bins_v and 0 <= a_bin < self.num_bins_a: - self.collected_data_counts_of_vel_acc[v_bin, a_bin] += 1 + self.acc_hist = [0.0] * 200 + self.vel_hist = [0.0] * 200 - if 0 <= v_bin < self.num_bins_v and 0 <= steer_bin < self.num_bins_steer: - self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 + def subscribe_collected_data_counts_of_vel_acc(self, msg): + rows = msg.layout.dim[0].size + cols = msg.layout.dim[1].size + self.collected_data_counts_of_vel_acc = np.array(msg.data).reshape((rows, cols)) - def timer_callback_plotter(self): - with self.lock: - self.collected_data_counts_of_vel_acc_for_plot = ( - self.collected_data_counts_of_vel_acc.copy() - ) - self.collected_data_counts_of_vel_steer_for_plot = ( - self.collected_data_counts_of_vel_steer.copy() - ) + def subscribe_collected_data_counts_of_vel_steer(self, msg): + rows = msg.layout.dim[0].size + cols = msg.layout.dim[1].size + self.collected_data_counts_of_vel_steer = np.array(msg.data).reshape((rows, cols)) - self.acc_hist_for_plot = self.acc_hist.copy() - self.vel_hist_for_plot = self.vel_hist.copy() + def subscribe_acc_hist(self, msg): + self.acc_hist = msg.data - self.v_bin_centers_for_plot = self.v_bin_centers - self.steer_bin_centers_for_plot = self.steer_bin_centers - self.a_bin_centers_for_plot = self.a_bin_centers + def subscribe_vel_hist(self, msg): + self.vel_hist = msg.data + def timer_callback_plotter(self): self.plot_data_collection_grid() - plt.pause(0.01) - - def timer_callback_counter(self): - if self._present_kinematic_state is not None and self._present_acceleration is not None: - # calculate steer - angular_z = self._present_kinematic_state.twist.twist.angular.z - wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - current_steer = arctan2( - wheel_base * angular_z, self._present_kinematic_state.twist.twist.linear.x - ) - - current_vel = self._present_kinematic_state.twist.twist.linear.x - current_acc = self._present_acceleration.accel.accel.linear.x - - # update velocity and acceleration bin if ego vehicle is moving - if self._present_kinematic_state.twist.twist.linear.x > 1e-3: - with self.lock: - self.count_observations( - current_vel, - current_acc, - current_steer, - ) - - self.acc_hist[:-1] = 1.0 * self.acc_hist[1:] - self.acc_hist[-1] = current_acc - self.vel_hist[:-1] = 1.0 * self.vel_hist[1:] - self.vel_hist[-1] = current_vel + plt.pause(0.1) def plot_data_collection_grid(self): self.axs[0].cla() - self.axs[0].scatter(self.acc_hist_for_plot, self.vel_hist_for_plot) - self.axs[0].plot(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].scatter(self.acc_hist, self.vel_hist) + self.axs[0].plot(self.acc_hist, self.vel_hist) self.axs[0].set_xlim([-2.0, 2.0]) self.axs[0].set_ylim([0.0, self.v_max + 1.0]) self.axs[0].set_xlabel("Acceleration") @@ -258,11 +108,11 @@ def plot_data_collection_grid(self): self.axs[1].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_acc_for_plot.T, + self.collected_data_counts_of_vel_acc.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers_for_plot, 2), - yticklabels=np.round(self.a_bin_centers_for_plot, 2), + xticklabels=np.round(self.v_bin_centers, 2), + yticklabels=np.round(self.a_bin_centers, 2), ax=self.axs[1], linewidths=0.1, linecolor="gray", @@ -277,11 +127,11 @@ def plot_data_collection_grid(self): self.axs[2].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_steer_for_plot.T, + self.collected_data_counts_of_vel_steer.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers_for_plot, 2), - yticklabels=np.round(self.steer_bin_centers_for_plot, 2), + xticklabels=np.round(self.v_bin_centers, 2), + yticklabels=np.round(self.steer_bin_centers, 2), ax=self.axs[2], linewidths=0.1, linecolor="gray", @@ -293,8 +143,6 @@ def plot_data_collection_grid(self): self.fig.canvas.draw() - plt.pause(0.01) - def main(args=None): rclpy.init(args=args) diff --git a/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py new file mode 100755 index 00000000..ac29dbc4 --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_rosbag_record.py @@ -0,0 +1,207 @@ +#!/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 datetime import datetime +from functools import partial +import os + +from ament_index_python.packages import get_package_share_directory +from autoware_adapi_v1_msgs.msg import OperationModeState +from autoware_vehicle_msgs.msg import ControlModeReport +import rclpy +from rclpy.node import Node +from rclpy.serialization import serialize_message +from rosbag2_py import ConverterOptions +from rosbag2_py import SequentialWriter +from rosbag2_py import StorageOptions +from rosbag2_py._storage import TopicMetadata +from rosidl_runtime_py.utilities import get_message +import yaml + + +class MessageWriter: + def __init__(self, topics, node): + self.topics = topics + self.node = node + self.message_writer = None + self.message_subscriptions_ = [] + + def create_writer(self): + self.message_writer = SequentialWriter() + + def subscribe_topics(self): + topic_type_list = [] + unsubscribed_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 + + # Generate a unique directory and filename based on the current time for the rosbag file + now = datetime.now() + formatted_time = now.strftime("%Y_%m_%d-%H_%M_%S") + bag_dir = f"./rosbag2_{formatted_time}" + storage_options = StorageOptions(uri=bag_dir, storage_id="sqlite3") + converter_options = ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + + # 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 + + # 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) + + def get_topic_type(self, topic_name): + # Get the list of topics and their types from the ROS node + topic_names_and_types = self.node.get_topic_names_and_types() + for name, types in topic_names_and_types: + if name == topic_name: + return types[0] + return None + + def start_record(self): + # Subscribe to the topics and start recording messages + for topic_name in self.topics: + topic_type = self.get_topic_type(topic_name) + if topic_type: + msg_module = get_message(topic_type) + subscription_ = self.node.create_subscription( + 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") + + # call back function called in start recording + def callback_write_message(self, topic_name, message): + try: + serialized_message = serialize_message(message) + current_time = rclpy.clock.Clock().now() + self.message_writer.write(topic_name, serialized_message, current_time.nanoseconds) + except Exception as e: + self.node.get_logger().error(f"Failed to write message: {e}") + + 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") + + +class DataCollectingRosbagRecord(Node): + def __init__(self): + super().__init__("data_collecting_rosbag_record") + package_share_directory = get_package_share_directory("control_data_collecting_tool") + topic_file_path = os.path.join(package_share_directory, "config", "topics.yaml") + with open(topic_file_path, "r") as file: + topic_data = yaml.safe_load(file) + self.topics = topic_data["topics"] + self.writer = MessageWriter(self.topics, self) + self.subscribed = False + self.recording = False + + self.present_operation_mode_ = None + self.operation_mode_subscription_ = self.create_subscription( + OperationModeState, + "/system/operation_mode/state", + self.subscribe_operation_mode, + 10, + ) + + self._present_control_mode_ = None + self.control_mode_subscription_ = self.create_subscription( + ControlModeReport, + "/vehicle/status/control_mode", + self.subscribe_control_mode, + 10, + ) + + self.timer_period_callback = 1.0 + self.timer_callback = self.create_timer( + self.timer_period_callback, + self.record_message, + ) + + def subscribe_operation_mode(self, msg): + self.present_operation_mode_ = msg.mode + + def subscribe_control_mode(self, msg): + self._present_control_mode_ = msg.mode + + def record_message(self): + # Start subscribing to topics and recording if the operation mode is 3(LOCAL) and control mode is 1(AUTONOMOUS) + if ( + self.present_operation_mode_ == 3 + and self._present_control_mode_ == 1 + and not self.subscribed + and not self.recording + ): + self.writer.create_writer() + self.writer.subscribe_topics() + self.subscribed = True + + # Start recording if topics are subscribed and the operation mode is 3(LOCAL) + if ( + self.present_operation_mode_ == 3 + and self._present_control_mode_ == 1 + and self.subscribed + and not self.recording + ): + self.writer.start_record() + self.recording = True + + # Stop recording if the operation mode changes from 3(LOCAL) + if ( + (self.present_operation_mode_ != 3 or self._present_control_mode_ != 1) + and self.subscribed + and self.recording + ): + self.writer.stop_record() + self.subscribed = False + self.recording = False + + +def main(args=None): + rclpy.init(args=args) + + data_collecting_rosbag_record = DataCollectingRosbagRecord() + rclpy.spin(data_collecting_rosbag_record) + data_collecting_rosbag_record.destroy_node() + + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 d8c396ac..cbc86d47 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -16,21 +16,19 @@ from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint -from geometry_msgs.msg import AccelWithCovarianceStamped +from data_collecting_base_node import DataCollectingBaseNode from geometry_msgs.msg import Point from geometry_msgs.msg import PolygonStamped import matplotlib.pyplot as plt -from nav_msgs.msg import Odometry import numpy as np -from numpy import arctan2 from numpy import cos from numpy import pi from numpy import sin from rcl_interfaces.msg import ParameterDescriptor import rclpy -from rclpy.node import Node from scipy.spatial.transform import Rotation as R -import seaborn as sns +from std_msgs.msg import Bool +from std_msgs.msg import Int32MultiArray from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray @@ -54,6 +52,10 @@ 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, @@ -96,7 +98,6 @@ def get_eight_course_trajectory_points( break if 0 <= t and t <= OB: - print(t) x[i] = (b / 2 - (1.0 - np.sqrt(3) / 2) * a) * t / OB y[i] = a * t / (2 * OB) yaw[i] = θB @@ -120,7 +121,6 @@ def get_eight_course_trajectory_points( 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) @@ -131,7 +131,6 @@ def get_eight_course_trajectory_points( 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 @@ -141,7 +140,6 @@ def get_eight_course_trajectory_points( 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) @@ -275,7 +273,8 @@ def get_u_shaped_return_course_trajectory_points( return np.array([x, y]).T, yaw, curve[:i_end], parts, achievement_rates -class DataCollectingTrajectoryPublisher(Node): +# inherits from DataCollectingBaseNode +class DataCollectingTrajectoryPublisher(DataCollectingBaseNode): def __init__(self): super().__init__("data_collecting_trajectory_publisher") @@ -287,66 +286,6 @@ def __init__(self): ), ) - self.declare_parameter( - "NUM_BINS_V", - 10, - ParameterDescriptor(description="Number of bins of velocity in heatmap"), - ) - - self.declare_parameter( - "NUM_BINS_STEER", - 10, - ParameterDescriptor(description="Number of bins of steer in heatmap"), - ) - - self.declare_parameter( - "NUM_BINS_A", - 10, - ParameterDescriptor(description="Number of bins of acceleration in heatmap"), - ) - - self.declare_parameter( - "V_MIN", - -1.0, - ParameterDescriptor(description="Maximum velocity in heatmap [m/s]"), - ) - - self.declare_parameter( - "V_MAX", - 1.0, - ParameterDescriptor(description="Minimum steer in heatmap [m/s]"), - ) - - self.declare_parameter( - "STEER_MIN", - -1.0, - ParameterDescriptor(description="Maximum steer in heatmap [rad]"), - ) - - self.declare_parameter( - "STEER_MAX", - 1.0, - ParameterDescriptor(description="Maximum steer in heatmap [rad]"), - ) - - self.declare_parameter( - "A_MIN", - -1.0, - ParameterDescriptor(description="Minimum acceleration in heatmap [m/ss]"), - ) - - self.declare_parameter( - "A_MAX", - 1.0, - ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), - ) - - self.declare_parameter( - "wheel_base", - 2.79, # sample_vehicle_launch/sample_vehicle_description/config/vehicle_info.param.yaml - ParameterDescriptor(description="Wheel base [m]"), - ) - self.declare_parameter( "acc_kp", 1.0, @@ -385,7 +324,7 @@ def __init__(self): self.declare_parameter( "mov_ave_window", - 100, + 50, ParameterDescriptor(description="Moving average smoothing window size"), ) @@ -419,6 +358,30 @@ def __init__(self): ), ) + self.declare_parameter( + "COLLECTING_DATA_V_MIN", + 0.0, + ParameterDescriptor(description="Minimum velocity for data collection [m/s]"), + ) + + self.declare_parameter( + "COLLECTING_DATA_V_MAX", + 11.5, + ParameterDescriptor(description="Maximum velocity for data collection [m/s]"), + ) + + self.declare_parameter( + "COLLECTING_DATA_A_MIN", + -1.0, + ParameterDescriptor(description="Minimum velocity for data collection [m/ss]"), + ) + + self.declare_parameter( + "COLLECTING_DATA_A_MAX", + 1.0, + ParameterDescriptor(description="Maximum velocity for data collection [m/ss]"), + ) + self.trajectory_for_collecting_data_pub_ = self.create_publisher( Trajectory, "/data_collecting_trajectory", @@ -431,17 +394,9 @@ def __init__(self): 1, ) - self.sub_odometry_ = self.create_subscription( - Odometry, - "/localization/kinematic_state", - self.onOdometry, - 1, - ) - - self.sub_acceleration_ = self.create_subscription( - AccelWithCovarianceStamped, - "/localization/acceleration", - self.onAcceleration, + self.pub_stop_request_ = self.create_publisher( + Bool, + "/data_collecting_stop_request", 1, ) @@ -456,37 +411,6 @@ def __init__(self): # set course name self.COURSE_NAME = self.get_parameter("COURSE_NAME").value - # velocity and acceleration grid - # velocity and steer grid - self.num_bins_v = self.get_parameter("NUM_BINS_V").get_parameter_value().integer_value - self.num_bins_steer = ( - self.get_parameter("NUM_BINS_STEER").get_parameter_value().integer_value - ) - self.num_bins_a = self.get_parameter("NUM_BINS_A").get_parameter_value().integer_value - self.v_min, self.v_max = ( - self.get_parameter("V_MIN").get_parameter_value().double_value, - self.get_parameter("V_MAX").get_parameter_value().double_value, - ) - self.steer_min, self.steer_max = ( - self.get_parameter("STEER_MIN").get_parameter_value().double_value, - self.get_parameter("STEER_MAX").get_parameter_value().double_value, - ) - self.a_min, self.a_max = ( - self.get_parameter("A_MIN").get_parameter_value().double_value, - self.get_parameter("A_MAX").get_parameter_value().double_value, - ) - - self.collected_data_counts_of_vel_acc = np.zeros((self.num_bins_v, self.num_bins_a)) - self.collected_data_counts_of_vel_steer = np.zeros((self.num_bins_v, self.num_bins_steer)) - - 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 - self.trajectory_parts = None self.trajectory_achievement_rates = None @@ -501,9 +425,6 @@ def __init__(self): self.on_line_vel_flag = True self.deceleration_rate = 0.6 - self.vel_hist = np.zeros(200) - self.acc_hist = np.zeros(200) - 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) @@ -527,14 +448,53 @@ def __init__(self): ) self.one_round_progress_rate = None - self.vel_noise_list = [] - def onOdometry(self, msg): - self._present_kinematic_state = msg + 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 + ) + + self.collected_data_counts_of_vel_acc_subscription_ = self.create_subscription( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_acc", + self.subscribe_collected_data_counts_of_vel_acc, + 10, + ) + self.collected_data_counts_of_vel_acc_subscription_ + + self.collected_data_counts_of_vel_steer_subscription_ = self.create_subscription( + Int32MultiArray, + "/control_data_collecting_tools/collected_data_counts_of_vel_steer", + self.subscribe_collected_data_counts_of_vel_steer, + 10, + ) + self.collected_data_counts_of_vel_steer_subscription_ + + def subscribe_collected_data_counts_of_vel_acc(self, msg): + rows = msg.layout.dim[0].size + cols = msg.layout.dim[1].size + self.collected_data_counts_of_vel_acc = np.array(msg.data).reshape((rows, cols)) - def onAcceleration(self, msg): - self._present_acceleration = msg + def subscribe_collected_data_counts_of_vel_steer(self, msg): + rows = msg.layout.dim[0].size + cols = msg.layout.dim[1].size + self.collected_data_counts_of_vel_steer = np.array(msg.data).reshape((rows, cols)) def onDataCollectingArea(self, msg): self._data_collecting_area_polygon = msg @@ -546,17 +506,11 @@ def get_target_velocity(self, 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 - current_acc = self._present_acceleration.accel.accel.linear.x 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.acc_hist[:-1] = 1.0 * self.acc_hist[1:] - self.acc_hist[-1] = current_acc - self.vel_hist[:-1] = 1.0 * self.vel_hist[1:] - self.vel_hist[-1] = current_vel - max_lateral_accel = ( self.get_parameter("max_lateral_accel").get_parameter_value().double_value ) @@ -596,8 +550,8 @@ def get_target_velocity(self, nearestIndex): (-1 + N_V, -3 + N_A), ] - for i in range(0, N_V): - for j in range(0, 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 @@ -623,11 +577,11 @@ def get_target_velocity(self, nearestIndex): 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.15 + self.deceleration_rate = 0.55 + 0.10 elif self.target_vel_on_line > self.v_max / 2.0: - self.deceleration_rate = 0.65 + 0.25 + self.deceleration_rate = 0.65 + 0.10 else: - self.deceleration_rate = 0.85 + 0.1 + 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: @@ -915,64 +869,39 @@ def updateNominalTargetTrajectory(self): self.get_logger().info("update nominal target trajectory") - def count_observations(self, v, a, steer): - v_bin = np.digitize(v, self.v_bins) - 1 - steer_bin = np.digitize(steer, self.steer_bins) - 1 - a_bin = np.digitize(a, self.a_bins) - 1 - - if 0 <= v_bin < self.num_bins_v and 0 <= a_bin < self.num_bins_a: - self.collected_data_counts_of_vel_acc[v_bin, a_bin] += 1 - - if 0 <= v_bin < self.num_bins_v and 0 <= steer_bin < self.num_bins_steer: - self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 - - def plot_data_collection_grid(self): - self.axs[1].cla() - self.axs[1].scatter(self.acc_hist, self.vel_hist) - self.axs[1].plot(self.acc_hist, self.vel_hist) - self.axs[1].set_xlim([-2.0, 2.0]) - self.axs[1].set_ylim([0.0, self.v_max + 1.0]) - self.axs[1].set_xlabel("Acceleration") - self.axs[1].set_ylabel("Velocity") - - # update collected acceleration and velocity grid - for collection in self.axs[2].collections: - if collection.colorbar is not None: - collection.colorbar.remove() - self.axs[2].cla() - - self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_acc.T, - annot=True, - cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers, 2), - yticklabels=np.round(self.a_bin_centers, 2), - ax=self.axs[2], - linewidths=0.1, - linecolor="gray", + 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) + ] ) - self.axs[2].set_xlabel("Velocity bins") - self.axs[2].set_ylabel("Acceleration bins") - - for collection in self.axs[3].collections: - if collection.colorbar is not None: - collection.colorbar.remove() - self.axs[3].cla() - - self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_steer.T, - annot=True, - cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers, 2), - yticklabels=np.round(self.steer_bin_centers, 2), - ax=self.axs[3], - linewidths=0.1, - linecolor="gray", + 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] - self.axs[3].set_xlabel("Velocity bins") - self.axs[3].set_ylabel("Steer bins") + area_ABCD = computeTriangleArea(A, B, C) + computeTriangleArea(C, D, A) + + area_PAB = computeTriangleArea(P, A, B) + area_PBC = computeTriangleArea(P, B, C) + area_PCD = computeTriangleArea(P, C, D) + area_PDA = computeTriangleArea(P, D, A) + + if area_PAB + area_PBC + area_PCD + area_PDA > area_ABCD * 1.001: + return False + else: + return True def timer_callback_traj(self): if ( @@ -980,21 +909,6 @@ def timer_callback_traj(self): and self._present_acceleration is not None and self.trajectory_position_data is not None ): - # calculate steer - angular_z = self._present_kinematic_state.twist.twist.angular.z - wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value - steer = arctan2( - wheel_base * angular_z, self._present_kinematic_state.twist.twist.linear.x - ) - - # update velocity and acceleration bin if ego vehicle is moving - if self._present_kinematic_state.twist.twist.linear.x > 1e-3: - self.count_observations( - self._present_kinematic_state.twist.twist.linear.x, - self._present_acceleration.accel.accel.linear.x, - steer, - ) - # [0] update nominal target trajectory if changing related ros2 params target_longitudinal_velocity = ( self.get_parameter("target_longitudinal_velocity") @@ -1009,7 +923,7 @@ def timer_callback_traj(self): > 1e-6 or window != self.current_window ): - True # self.updateNominalTargetTrajectory() + self.updateNominalTargetTrajectory() # [1] receive observation from topic present_position = np.array( @@ -1034,7 +948,12 @@ def timer_callback_traj(self): self._present_kinematic_state.twist.twist.linear.z, ] ) - present_yaw = getYaw(present_orientation) + + if np.linalg.norm(present_orientation) < 1e-6: + present_yaw = self.previous_yaw + else: + present_yaw = getYaw(present_orientation) + self.previous_yaw = present_yaw # [2] get whole trajectory data trajectory_position_data = self.trajectory_position_data.copy() @@ -1332,6 +1251,11 @@ def timer_callback_traj(self): 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): + msg = Bool() + msg.data = True + self.pub_stop_request_.publish(msg) if debug_matplotlib_plot_flag: self.axs[0].cla() @@ -1388,8 +1312,6 @@ def timer_callback_traj(self): self.axs[0].set_ylabel("longitudinal_velocity [m/s]") self.axs[0].legend(fontsize=8) - self.plot_data_collection_grid() - self.fig.canvas.draw() plt.pause(0.01) diff --git a/control_data_collecting_tool/scripts/rosbag_play.py b/control_data_collecting_tool/scripts/rosbag_play.py new file mode 100644 index 00000000..7ed4ed3f --- /dev/null +++ b/control_data_collecting_tool/scripts/rosbag_play.py @@ -0,0 +1,96 @@ +#!/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 sqlite3 + +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + + +class db3SingleTopic: + def __init__(self, db3_file, topic_id): + self.conn = sqlite3.connect(db3_file) + self.c = self.conn.cursor() + self.c.execute("SELECT * from messages WHERE topic_id = ?", (topic_id,)) + self.is_open = True + + def __del__(self): + self.conn.close() + + # Fetch the next row from the SQLite query result + def fetch_row(self): + return self.c.fetchone() + + +class db3Reader: + def __init__(self, db3_file): + self.db3_file = db3_file + + self.db3_dict = {} + + def __del__(self): + # Clean up by deleting the topic-specific objects and closing any open database connections + for topic_db3 in self.db3_dict.values(): + del topic_db3["topic_db3"] + + # Load topic information from the .db3 file + def load_db3(self, target_topic_name): + # Try to establish a connection to the .db3 SQLite database and retrieve topics + try: + conn = sqlite3.connect(self.db3_file) + c = conn.cursor() + c.execute("SELECT * from({})".format("topics")) + topics = c.fetchall() + + except sqlite3.Error: + return False + + # Create a dictionary that maps topic names to their respective ID and message type + topic_types = { + topics[i][1]: {"topic_id": i + 1, "topic_type": get_message(topics[i][2])} + for i in range(len(topics)) + } + + # Check if the target topic is included in the database + target_topic_included = True + if target_topic_name not in list(topic_types.keys()): + target_topic_included = False + conn.close() + + # If the target topic is not found, return False + if not target_topic_included: + return target_topic_included + + # if the target topic is included, establish a connection to an SQLite database to read the target topic + topic_type = topic_types[target_topic_name]["topic_type"] + topic_db3 = db3SingleTopic(self.db3_file, topic_types[target_topic_name]["topic_id"]) + self.db3_dict[target_topic_name] = {"topic_type": topic_type, "topic_db3": topic_db3} + + # Successfully loaded the target topic + return True + + # Fetch a deserialized single message for the specified topic + def read_msg(self, target_topic_name): + msg = None + row = self.db3_dict[target_topic_name]["topic_db3"].fetch_row() + + if row is None: + return msg + + topic_type = self.db3_dict[target_topic_name]["topic_type"] + msg = deserialize_message(row[3], topic_type) + + return msg