From 9886b079e3798b0d13db11aac3374d0eabe3d1c2 Mon Sep 17 00:00:00 2001 From: bbegooo Date: Sun, 22 Mar 2026 09:19:59 +0100 Subject: [PATCH 01/13] feat: change paremeters 1 acc run --- src/common/common_meta/launch/acceleration_launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/common_meta/launch/acceleration_launch.py b/src/common/common_meta/launch/acceleration_launch.py index 5dcc1187..ba6dcb66 100644 --- a/src/common/common_meta/launch/acceleration_launch.py +++ b/src/common/common_meta/launch/acceleration_launch.py @@ -43,15 +43,15 @@ def generate_launch_description(): 'min_y_clip': -20.0}]), create_node(pkg='acc_planning', params=[{'target_speed': 10.0, - 'max_acc': 4.0, - 'max_dec': 4.0, + 'max_acc': 5.0, + 'max_dec': 5.0, 'track_length': 75.0, 'finish_offset': 1.0}]), create_node(pkg='controller', params=[{'trajectory_topic': "/acc_planning/trajectory", 'min_cmd': -5.0, 'max_cmd': 5.0, - 'look_ahead_distance': 9.0}]), + 'look_ahead_distance': 8.0}]), create_node(pkg='graph_slam'), create_node(pkg='car_state', params=[{'simulation': False, From dc579d796bc7c441a506731722b13355e2be1869 Mon Sep 17 00:00:00 2001 From: root Date: Sun, 22 Mar 2026 09:18:18 +0000 Subject: [PATCH 02/13] refactor: change launch --- src/common/common_meta/launch/acceleration_launch.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/common/common_meta/launch/acceleration_launch.py b/src/common/common_meta/launch/acceleration_launch.py index ba6dcb66..972ff437 100644 --- a/src/common/common_meta/launch/acceleration_launch.py +++ b/src/common/common_meta/launch/acceleration_launch.py @@ -27,7 +27,8 @@ def generate_launch_description(): ) return LaunchDescription([ - create_node(pkg='pc_supervisor', params=[{'mission_name': 'acceleration', 'sim_mode': False}]), + rosbag_record, + # create_node(pkg='pc_supervisor', params=[{'mission_name': 'acceleration', 'sim_mode': False}]), IncludeLaunchDescription( PythonLaunchDescriptionSource(rslidar_launch)), # IncludeLaunchDescription( @@ -43,15 +44,15 @@ def generate_launch_description(): 'min_y_clip': -20.0}]), create_node(pkg='acc_planning', params=[{'target_speed': 10.0, - 'max_acc': 5.0, - 'max_dec': 5.0, + 'max_acc': 4.0, + 'max_dec': 4.0, 'track_length': 75.0, 'finish_offset': 1.0}]), create_node(pkg='controller', params=[{'trajectory_topic': "/acc_planning/trajectory", 'min_cmd': -5.0, 'max_cmd': 5.0, - 'look_ahead_distance': 8.0}]), + 'look_ahead_distance': 9.0}]), create_node(pkg='graph_slam'), create_node(pkg='car_state', params=[{'simulation': False, @@ -63,7 +64,6 @@ def generate_launch_description(): # '/launch/foxglove_bridge_launch.xml' # ]) # ), - # rosbag_record, Node(package='common_meta', executable='param_dumper_exec', name='param_dumper'), ]) From b89044c84b1474aa0a41d7a7669da890ef99b4bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ignacio=20S=C3=A1nchez?= Date: Sun, 22 Mar 2026 10:53:10 +0100 Subject: [PATCH 03/13] fix: fix error when initializin empty vertex --- src/slam/graph_slam/src/graph_slam_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 4fa19b1d..b6acc555 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -983,7 +983,8 @@ g2o::VertexSE2* GraphSlam::get_last_valid_pose() { return pose_vertices_[i]; } } - return nullptr; + auto v = new g2o::VertexSE2(); + return v; } From 713b0ea9ab81954ef35d6c51afcee00e0763b0f3 Mon Sep 17 00:00:00 2001 From: root Date: Sun, 22 Mar 2026 10:17:48 +0000 Subject: [PATCH 04/13] fix: remove slow straight cropping --- src/common/common_meta/launch/acceleration_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/common_meta/launch/acceleration_launch.py b/src/common/common_meta/launch/acceleration_launch.py index 972ff437..b6039723 100644 --- a/src/common/common_meta/launch/acceleration_launch.py +++ b/src/common/common_meta/launch/acceleration_launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): create_node(pkg='can_interface'), create_node(pkg='epos_interface'), create_node(pkg='perception', - params=[{'slow_straight_cropping': True, + params=[{'slow_straight_cropping': False, 'max_x_fov': 50.0, 'max_y_fov': 7.0, 'max_x_clip': 150.0, From b03b8cb5139b968f45dad6938edd0b1e9798901c Mon Sep 17 00:00:00 2001 From: ART25D-KATANA Date: Sun, 22 Mar 2026 11:24:08 +0100 Subject: [PATCH 05/13] fix: change to artificial timestamp --- src/perception/config/perception_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index a9700455..fd9f0019 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -15,7 +15,7 @@ perception: clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing - use_artificial_timestamp: false # Use the node's current time as timestamp instead of the one from the LiDAR messages + use_artificial_timestamp: true # Use the node's current time as timestamp instead of the one from the LiDAR messages # Deskewing deskewing: true # Motion compensation within each LiDAR scan using point timestamps From 2d8a46fae65aec6dc0088da6ccb1b3aafda9c67f Mon Sep 17 00:00:00 2001 From: jmlp05 Date: Sun, 22 Mar 2026 12:08:40 +0100 Subject: [PATCH 06/13] feat: control steering with extensometer --- .../include/epos_interface/epos_interface_node.hpp | 1 + src/interfaces/epos_interface/src/epos_interface_node.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp b/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp index bce4a6b7..47df2edb 100644 --- a/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp +++ b/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp @@ -51,6 +51,7 @@ class EPOS_interface : public rclcpp::Node bool steer_check_; // Flag to indicate if steering is allowed bool started_; // Flag to indicate if the movement has started double ext_time_; // Time of the last extensometer reading + double ext_init_pos_; // Timers rclcpp::TimerBase::SharedPtr timer_; diff --git a/src/interfaces/epos_interface/src/epos_interface_node.cpp b/src/interfaces/epos_interface/src/epos_interface_node.cpp index ebe18835..db41c36f 100644 --- a/src/interfaces/epos_interface/src/epos_interface_node.cpp +++ b/src/interfaces/epos_interface/src/epos_interface_node.cpp @@ -95,7 +95,7 @@ void EPOS_interface::on_timer() double epos_cmd; if (kUseExtensometer) { - epos_cmd = delta_cmd_ - ext_pos_; + epos_cmd = delta_cmd_ - ext_init_pos_ + KP*(delta_cmd_ - ext_pos_); } else { epos_cmd = delta_cmd_; } @@ -116,9 +116,9 @@ void EPOS_interface::cmd_callback(const common_msgs::msg::Cmd::SharedPtr msg) void EPOS_interface::extensometer_callback(const std_msgs::msg::Float32::SharedPtr msg){ // Get smoothed extensometer value until movement starts - if (started_) return; ext_pos_ = 0.9*msg->data + 0.1*ext_pos_; ext_time_ = this->now().seconds(); + if(!started_) ext_init_pos_ = ext_pos_; if (std::abs(msg->data) > MAX_STEER){ if (kDebug) RCLCPP_ERROR(this->get_logger(), "Extensometer value out of range: %f", msg->data); From 3869af382cdf6788b5a7c3b80ff658a0baa095d8 Mon Sep 17 00:00:00 2001 From: bbegooo Date: Sun, 22 Mar 2026 13:48:26 +0100 Subject: [PATCH 07/13] feat: change parameters 1 trackdrive run --- .../common_meta/launch/acceleration_launch.py | 2 +- .../common_meta/launch/trackdrive_launch.py | 19 ++++++++++++++++--- src/perception/config/perception_config.yaml | 6 +++--- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/common/common_meta/launch/acceleration_launch.py b/src/common/common_meta/launch/acceleration_launch.py index ba6dcb66..7dd2a850 100644 --- a/src/common/common_meta/launch/acceleration_launch.py +++ b/src/common/common_meta/launch/acceleration_launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): create_node(pkg='epos_interface'), create_node(pkg='perception', params=[{'slow_straight_cropping': True, - 'max_x_fov': 50.0, + 'max_x_fov': 50.0, 'max_y_fov': 7.0, 'max_x_clip': 150.0, 'max_y_clip': 20.0, diff --git a/src/common/common_meta/launch/trackdrive_launch.py b/src/common/common_meta/launch/trackdrive_launch.py index 7e065e03..763e0dd5 100644 --- a/src/common/common_meta/launch/trackdrive_launch.py +++ b/src/common/common_meta/launch/trackdrive_launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): create_node(pkg='epos_interface'), create_node(pkg='perception'), create_node(pkg='path_planning', - params=[{'v_max': 7.0, + params=[{'v_max': 10.0, 'ax_max': 5.0, 'ay_max': 5.0}]), create_node(pkg='controller', @@ -41,6 +41,19 @@ def generate_launch_description(): 'use_optimized_trajectory': True, 'first_lap_steer_control': "PP", 'optimized_steer_control': "MPC", + 'use_variable_look_ahead_distance' : True, + 'min_look_ahead_distance' : 4.0, + 'max_look_ahead_distance ' : 8.0, + 'lambda_vx' : 0.55, + 'lambda_k' : 0.5, + 'cost_lateral_error': 15.0, + 'cost_angular_error':5.0, + 'cost_steering_delta' : 2000.0, + 'store_map' : True, + 'store_opt_traj': True, + 'load_map' : False, + 'load_opt_traj': False, + 'trackdrive_laps' : 2, 'auto_start': False}]), create_node(pkg='graph_slam', params=[{'finish_line_offset': 10.0, @@ -55,8 +68,8 @@ def generate_launch_description(): params=[{'v_max': 10.0, 'd_min': 1.3, 'mu_y': 0.6, - 'mu_throttle': 0.5, - 'mu_brake': 0.3}]), + 'mu_throttle': 0.4, + 'mu_brake': 0.4}]), create_node(pkg='visualization'), # IncludeLaunchDescription( # AnyLaunchDescriptionSource([ diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index a9700455..cb8416ab 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -30,10 +30,10 @@ perception: # Clipping parameters clipping: true # Enable clipping of the point cloud - max_x_clip: 50.0 + max_x_clip: 40.0 max_y_clip: 5.0 - min_x_clip: -30.0 - min_y_clip: -80.0 + min_x_clip: -25.0 + min_y_clip: -60.0 max_z_clip: 3.0 min_z_clip: -10.0 From 7be4642c4aec9e15d4b14c7457fe545a176c8ca6 Mon Sep 17 00:00:00 2001 From: bbegooo Date: Sun, 22 Mar 2026 14:07:15 +0100 Subject: [PATCH 08/13] feat: change params 2 tk run --- src/common/common_meta/launch/trackdrive_launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/common_meta/launch/trackdrive_launch.py b/src/common/common_meta/launch/trackdrive_launch.py index 763e0dd5..0900923b 100644 --- a/src/common/common_meta/launch/trackdrive_launch.py +++ b/src/common/common_meta/launch/trackdrive_launch.py @@ -30,9 +30,9 @@ def generate_launch_description(): create_node(pkg='epos_interface'), create_node(pkg='perception'), create_node(pkg='path_planning', - params=[{'v_max': 10.0, - 'ax_max': 5.0, - 'ay_max': 5.0}]), + params=[{'v_max': 7.0, + 'ax_max': 3.5, + 'ay_max': 3.5}]), create_node(pkg='controller', params=[{'look_ahead_distance': 5.0, 'optimized_look_ahead_distance': 7.0, From 096bb3c14244b257740c48b83eb7297d401e2202 Mon Sep 17 00:00:00 2001 From: bbegooo Date: Sun, 22 Mar 2026 14:22:05 +0100 Subject: [PATCH 09/13] fix: deactivate extensometer_control --- .../include/epos_interface/epos_interface_node.hpp | 3 +-- src/interfaces/epos_interface/src/epos_interface_node.cpp | 8 +++----- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp b/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp index 47df2edb..ff7ffab2 100644 --- a/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp +++ b/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp @@ -51,7 +51,6 @@ class EPOS_interface : public rclcpp::Node bool steer_check_; // Flag to indicate if steering is allowed bool started_; // Flag to indicate if the movement has started double ext_time_; // Time of the last extensometer reading - double ext_init_pos_; // Timers rclcpp::TimerBase::SharedPtr timer_; @@ -88,4 +87,4 @@ class EPOS_interface : public rclcpp::Node * @brief Function to disable and close the EPOS4 controller. */ void clean_and_close(); -}; +}; \ No newline at end of file diff --git a/src/interfaces/epos_interface/src/epos_interface_node.cpp b/src/interfaces/epos_interface/src/epos_interface_node.cpp index db41c36f..f7256b67 100644 --- a/src/interfaces/epos_interface/src/epos_interface_node.cpp +++ b/src/interfaces/epos_interface/src/epos_interface_node.cpp @@ -95,7 +95,7 @@ void EPOS_interface::on_timer() double epos_cmd; if (kUseExtensometer) { - epos_cmd = delta_cmd_ - ext_init_pos_ + KP*(delta_cmd_ - ext_pos_); + epos_cmd = delta_cmd_ - ext_pos_; } else { epos_cmd = delta_cmd_; } @@ -116,9 +116,9 @@ void EPOS_interface::cmd_callback(const common_msgs::msg::Cmd::SharedPtr msg) void EPOS_interface::extensometer_callback(const std_msgs::msg::Float32::SharedPtr msg){ // Get smoothed extensometer value until movement starts + if (started_) return; ext_pos_ = 0.9*msg->data + 0.1*ext_pos_; ext_time_ = this->now().seconds(); - if(!started_) ext_init_pos_ = ext_pos_; if (std::abs(msg->data) > MAX_STEER){ if (kDebug) RCLCPP_ERROR(this->get_logger(), "Extensometer value out of range: %f", msg->data); @@ -146,6 +146,4 @@ int main(int argc, char *argv[]) rclcpp::spin(EPOS_interface_node); rclcpp::shutdown(); return 0; -} - - +} \ No newline at end of file From 4b27899ba13aee63604c3b2c1c78c5519a863473 Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Sun, 22 Mar 2026 16:03:23 +0100 Subject: [PATCH 10/13] fix: correct trackdrive launch parameters --- .../common_meta/launch/trackdrive_launch.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/common/common_meta/launch/trackdrive_launch.py b/src/common/common_meta/launch/trackdrive_launch.py index 0900923b..6bf2fdca 100644 --- a/src/common/common_meta/launch/trackdrive_launch.py +++ b/src/common/common_meta/launch/trackdrive_launch.py @@ -49,27 +49,27 @@ def generate_launch_description(): 'cost_lateral_error': 15.0, 'cost_angular_error':5.0, 'cost_steering_delta' : 2000.0, - 'store_map' : True, - 'store_opt_traj': True, - 'load_map' : False, - 'load_opt_traj': False, - 'trackdrive_laps' : 2, 'auto_start': False}]), create_node(pkg='graph_slam', params=[{'finish_line_offset': 10.0, 'add_big_cones': False, 'x_big_cone': 7.85, - 'y_big_cone': 1.5}]), + 'y_big_cone': 1.5, + 'store_map' : True, + 'store_opt_traj': True, + 'load_map' : False, + 'load_opt_traj': False}]), create_node(pkg='car_state', params=[{'simulation': False, 'mission': 'trackdrive', - 'trackdrive_laps': 10}]), + 'trackdrive_laps': 2}]), create_node(pkg='trajectory_optimization', params=[{'v_max': 10.0, 'd_min': 1.3, 'mu_y': 0.6, 'mu_throttle': 0.4, - 'mu_brake': 0.4}]), + 'mu_brake': 0.4, + 'first_lap_optimization_method': "smoothing"}]), create_node(pkg='visualization'), # IncludeLaunchDescription( # AnyLaunchDescriptionSource([ From ebc9bf5b4392430f3632a8f23d54d4ef92d47454 Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Sun, 22 Mar 2026 16:29:29 +0100 Subject: [PATCH 11/13] feat: update parameters for whole trackdrive --- src/common/common_meta/launch/trackdrive_launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/common_meta/launch/trackdrive_launch.py b/src/common/common_meta/launch/trackdrive_launch.py index 6bf2fdca..f9e03d9f 100644 --- a/src/common/common_meta/launch/trackdrive_launch.py +++ b/src/common/common_meta/launch/trackdrive_launch.py @@ -62,14 +62,14 @@ def generate_launch_description(): create_node(pkg='car_state', params=[{'simulation': False, 'mission': 'trackdrive', - 'trackdrive_laps': 2}]), + 'trackdrive_laps': 10}]), create_node(pkg='trajectory_optimization', params=[{'v_max': 10.0, 'd_min': 1.3, - 'mu_y': 0.6, + 'mu_y': 0.4, 'mu_throttle': 0.4, 'mu_brake': 0.4, - 'first_lap_optimization_method': "smoothing"}]), + 'first_lap_optimization_method': "none"}]), create_node(pkg='visualization'), # IncludeLaunchDescription( # AnyLaunchDescriptionSource([ From 819679dede40ddde717747991f31b177dab33162 Mon Sep 17 00:00:00 2001 From: root Date: Sun, 22 Mar 2026 16:00:44 +0000 Subject: [PATCH 12/13] params: change clipping --- src/perception/config/perception_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index e0d2da63..255b4cac 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -33,7 +33,7 @@ perception: max_x_clip: 40.0 max_y_clip: 5.0 min_x_clip: -25.0 - min_y_clip: -60.0 + min_y_clip: -72.0 max_z_clip: 3.0 min_z_clip: -10.0 From b3810288bf86b491f9cd6d3dea8d4f371804b311 Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Sun, 22 Mar 2026 18:01:27 +0100 Subject: [PATCH 13/13] fix: change var LAD params --- src/common/common_meta/launch/trackdrive_launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/common/common_meta/launch/trackdrive_launch.py b/src/common/common_meta/launch/trackdrive_launch.py index f9e03d9f..841cc908 100644 --- a/src/common/common_meta/launch/trackdrive_launch.py +++ b/src/common/common_meta/launch/trackdrive_launch.py @@ -36,16 +36,16 @@ def generate_launch_description(): create_node(pkg='controller', params=[{'look_ahead_distance': 5.0, 'optimized_look_ahead_distance': 7.0, - 'min_cmd': -5.0, - 'max_cmd': 6.0, + 'min_cmd': -4.0, + 'max_cmd': 4.0, 'use_optimized_trajectory': True, 'first_lap_steer_control': "PP", 'optimized_steer_control': "MPC", 'use_variable_look_ahead_distance' : True, 'min_look_ahead_distance' : 4.0, 'max_look_ahead_distance ' : 8.0, - 'lambda_vx' : 0.55, - 'lambda_k' : 0.5, + 'lambda_vx' : 0.9, + 'lambda_k' : 1.0, 'cost_lateral_error': 15.0, 'cost_angular_error':5.0, 'cost_steering_delta' : 2000.0,