From 776e004d5a6db6251cc7050457340a43c44c405a Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Wed, 5 Nov 2025 16:06:03 +0000 Subject: [PATCH 01/10] first iteration running --- config/car/vehicle.yaml | 1 + config/slam/pacsim.yaml | 112 ++--- config/slam/vehicle.yaml | 61 ++- ext/as-integration | 2 +- ext/gtsam | 2 +- .../car_parameters/car_parameters.hpp | 7 +- .../include/common_lib/sensor_data/imu.hpp | 14 +- .../src/car_parameters/car_parameters.cpp | 3 + src/common_lib/src/sensor_data/imu.cpp | 8 +- src/motion_lib/CMakeLists.txt | 12 +- .../motion_lib/aero_model/base_aero_model.hpp | 22 - .../aero_model/default_aero_model.hpp | 8 - .../include/motion_lib/aero_model/map.hpp | 22 - .../include/motion_lib/car_parameters.hpp | 7 + .../base_load_transfer_model.hpp | 30 -- .../motion_lib/load_transfer_model/map.hpp | 23 - .../load_transfer_model/rigid_body_model.hpp | 16 - .../motion_lib/s2v_model/bicycle_model.hpp | 64 +++ .../include/motion_lib/s2v_model/map.hpp | 32 ++ .../s2v_model/no_rear_wss_bicycle_model.hpp | 62 +++ .../s2v_model/no_wss_bicycle_model.hpp | 62 +++ .../motion_lib/s2v_model/s2v_model.hpp | 69 +++ .../steering_model/base_steering_model.hpp | 23 - .../include/motion_lib/steering_model/map.hpp | 23 - .../parallel_front_steering.hpp | 14 - .../base_steering_motor_model.hpp | 25 -- .../motion_lib/steering_motor_model/map.hpp | 24 -- .../pid_steering_motor.hpp | 25 -- .../motion_lib/tire_model/base_tire_model.hpp | 27 -- .../include/motion_lib/tire_model/map.hpp | 22 - .../tire_model/pacejka_combined_slip.hpp | 9 - .../v2p_models/base_v2p_motion_model.hpp | 50 +-- .../constant_acceleration_turnrate_model.hpp | 50 +++ .../v2p_models/constant_velocity_model.hpp | 33 +- .../constant_velocity_turnrate_model.hpp | 33 +- .../include/motion_lib/v2p_models/map.hpp | 9 +- .../motion_lib/v2p_models/odometry_model.hpp | 52 +++ .../base_vel_process_model.hpp | 11 + .../vel_process_model/kinematic_bicycle.hpp | 0 .../vel_process_model/particle_model.hpp | 11 + .../src/aero_model/default_aero_model.cpp | 24 -- .../load_transfer_model/rigid_body_model.cpp | 41 -- .../src/s2v_model/bicycle_model.cpp | 135 ++++++ .../s2v_model/no_rear_wss_bicycle_model.cpp | 120 ++++++ .../src/s2v_model/no_wss_bicycle_model.cpp | 73 ++++ .../parallel_front_steering.cpp | 6 - .../pid_steering_motor.cpp | 24 -- .../src/tire_model/pacejka_combined_slip.cpp | 32 -- .../src/v2p_models/base_v2p_motion_model.cpp | 19 +- .../constant_acceleration_turnrate_model.cpp | 105 +++++ .../v2p_models/constant_velocity_model.cpp | 27 +- .../constant_velocity_turnrate_model.cpp | 89 ++-- .../src/v2p_models/odometry_model.cpp | 31 ++ .../src/vel_process_model/particle_model.cpp | 9 + .../test/s2v_models/bicycle_model_test.cpp} | 88 +++- ...stant_acceleration_turnrate_model_test.cpp | 400 +++++++++++++++++ .../vel_process_model/particle_model_test.cpp | 4 +- src/perception_sensor_lib/CMakeLists.txt | 14 +- .../data_association/map.hpp | 5 + .../nearest_neighbour_icp.hpp | 40 ++ .../landmark_filter/base_landmark_filter.hpp | 5 +- .../consecutive_counter_filter.hpp | 18 +- .../loop_closure/lap_counter.hpp | 12 +- .../loop_closure/loop_closure.hpp | 13 +- ...n_model.hpp => base_observation_model.hpp} | 0 .../base_observation_model.hpp | 43 -- .../ve_observation_model/map.hpp | 24 -- .../no_slip_bicycle_model.hpp | 29 -- .../nearest_neighbour_icp.cpp | 98 +++++ .../consecutive_counter_filter.cpp | 69 +-- .../src/loop_closure/lap_counter.cpp | 10 +- ...n_model.cpp => base_observation_model.cpp} | 2 +- .../no_slip_bicycle_model.cpp | 100 ----- .../nearest_neighbour_test.cpp | 172 ++++++++ .../consecutive_counter_filter_test.cpp | 68 ++- ...st.cpp => base_observation_model_test.cpp} | 4 +- src/slam/CMakeLists.txt | 7 +- src/slam/README.md | 117 ++++- src/slam/dependencies_install.sh | 0 src/slam/filter.py | 50 --- src/slam/format.py | 38 -- src/slam/include/adapter_slam/pacsim.hpp | 15 + src/slam/include/adapter_slam/vehicle.hpp | 18 +- src/slam/include/ros_node/slam_node.hpp | 41 +- .../include/slam_config/general_config.hpp | 65 ++- .../include/slam_solver/ekf_slam_solver.hpp | 17 +- .../factor_data_structures.hpp | 24 +- .../graph_slam_solver/graph_slam_instance.hpp | 73 +++- .../graph_slam_solver/graph_slam_solver.hpp | 83 +++- .../optimizer/base_optimizer.hpp | 24 ++ .../optimizer/isam2_optimizer.hpp | 46 +- .../graph_slam_solver/optimizer/map.hpp | 4 +- .../optimizer/normal_levenberg_optimizer.hpp | 26 ++ .../sliding_window_levenberg_optimizer.hpp | 27 ++ .../graph_slam_solver/pose_updater.hpp | 44 -- .../pose_updater/base_pose_updater.hpp | 97 +++++ .../difference_based_ready_pose_updater.hpp | 44 ++ .../pose_updater/double_pose_updater.hpp | 49 +++ .../graph_slam_solver/pose_updater/map.hpp | 25 ++ .../second_pose_input_trait.hpp | 27 ++ .../slam_solver/graph_slam_solver/utils.hpp | 4 +- src/slam/include/slam_solver/slam_solver.hpp | 24 +- .../solver_traits/imu_integrator_trait.hpp | 18 + .../solver_traits/node_controller_trait.hpp | 23 + .../odometry_integrator_trait.hpp | 20 + .../solver_traits/trajectory_calculator.hpp | 20 + .../velocities_integrator_trait.hpp | 20 + .../include/track_loader/track_loader.hpp | 8 - src/slam/maps/trackdrive.yaml | 403 ------------------ src/slam/package.xml | 3 + src/slam/printer.py | 37 -- src/slam/src/adapter_slam/pacsim.cpp | 37 +- src/slam/src/adapter_slam/vehicle.cpp | 57 +++ src/slam/src/main.cpp | 4 +- src/slam/src/ros_node/slam_node.cpp | 146 ++++--- src/slam/src/slam_config/general_config.cpp | 50 ++- src/slam/src/slam_solver/ekf_slam_solver.cpp | 25 +- .../graph_slam_solver/graph_slam_instance.cpp | 132 +++--- .../graph_slam_solver/graph_slam_solver.cpp | 331 +++++++++----- .../optimizer/isam2_optimizer.cpp | 57 ++- .../optimizer/normal_levenberg_optimizer.cpp | 4 + .../sliding_window_levenberg_optimizer.cpp | 68 ++- .../graph_slam_solver/pose_updater.cpp | 38 -- .../pose_updater/base_pose_updater.cpp | 110 +++++ .../difference_based_ready_pose_updater.cpp | 39 ++ .../pose_updater/double_pose_updater.cpp | 45 ++ .../slam_solver/graph_slam_solver/utils.cpp | 7 + src/slam/src/slam_solver/slam_solver.cpp | 10 +- src/slam/src/track_loader/track_loader.cpp | 7 - .../graph_slam_solver_test.cpp | 25 +- 130 files changed, 3753 insertions(+), 2013 deletions(-) delete mode 100644 src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/aero_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/car_parameters.hpp delete mode 100644 src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/load_transfer_model/map.hpp delete mode 100644 src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp create mode 100644 src/motion_lib/include/motion_lib/s2v_model/bicycle_model.hpp create mode 100644 src/motion_lib/include/motion_lib/s2v_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp create mode 100644 src/motion_lib/include/motion_lib/s2v_model/no_wss_bicycle_model.hpp create mode 100644 src/motion_lib/include/motion_lib/s2v_model/s2v_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/steering_model/map.hpp delete mode 100644 src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp delete mode 100644 src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/steering_motor_model/map.hpp delete mode 100644 src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp delete mode 100644 src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp delete mode 100644 src/motion_lib/include/motion_lib/tire_model/map.hpp delete mode 100644 src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp create mode 100644 src/motion_lib/include/motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp create mode 100644 src/motion_lib/include/motion_lib/v2p_models/odometry_model.hpp delete mode 100755 src/motion_lib/include/motion_lib/vel_process_model/kinematic_bicycle.hpp delete mode 100644 src/motion_lib/src/aero_model/default_aero_model.cpp delete mode 100644 src/motion_lib/src/load_transfer_model/rigid_body_model.cpp create mode 100644 src/motion_lib/src/s2v_model/bicycle_model.cpp create mode 100644 src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp create mode 100644 src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp delete mode 100644 src/motion_lib/src/steering_model/parallel_front_steering.cpp delete mode 100644 src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp delete mode 100644 src/motion_lib/src/tire_model/pacejka_combined_slip.cpp create mode 100644 src/motion_lib/src/v2p_models/constant_acceleration_turnrate_model.cpp create mode 100644 src/motion_lib/src/v2p_models/odometry_model.cpp rename src/{perception_sensor_lib/test/observation_model/ve_observation_model/no_slip_bicycle_model_test.cpp => motion_lib/test/s2v_models/bicycle_model_test.cpp} (70%) create mode 100644 src/motion_lib/test/v2p_models/constant_acceleration_turnrate_model_test.cpp create mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour_icp.hpp rename src/perception_sensor_lib/include/perception_sensor_lib/observation_model/{slam_observation_model/default_observation_model.hpp => base_observation_model.hpp} (100%) delete mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/base_observation_model.hpp delete mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/map.hpp delete mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp create mode 100644 src/perception_sensor_lib/src/data_association/nearest_neighbour_icp.cpp rename src/perception_sensor_lib/src/observation_model/{slam_observation_model/default_observation_model.cpp => base_observation_model.cpp} (97%) delete mode 100644 src/perception_sensor_lib/src/observation_model/ve_observation_model/no_slip_bicycle_model.cpp create mode 100644 src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp rename src/perception_sensor_lib/test/observation_model/{slam_observation_model/default_observation_model_test.cpp => base_observation_model_test.cpp} (94%) mode change 100755 => 100644 src/slam/dependencies_install.sh delete mode 100644 src/slam/filter.py delete mode 100644 src/slam/format.py delete mode 100644 src/slam/include/slam_solver/graph_slam_solver/pose_updater.hpp create mode 100644 src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp create mode 100644 src/slam/include/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp create mode 100644 src/slam/include/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.hpp create mode 100644 src/slam/include/slam_solver/graph_slam_solver/pose_updater/map.hpp create mode 100644 src/slam/include/slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp create mode 100644 src/slam/include/slam_solver/solver_traits/imu_integrator_trait.hpp create mode 100644 src/slam/include/slam_solver/solver_traits/node_controller_trait.hpp create mode 100644 src/slam/include/slam_solver/solver_traits/odometry_integrator_trait.hpp create mode 100644 src/slam/include/slam_solver/solver_traits/trajectory_calculator.hpp create mode 100644 src/slam/include/slam_solver/solver_traits/velocities_integrator_trait.hpp delete mode 100644 src/slam/maps/trackdrive.yaml delete mode 100644 src/slam/printer.py delete mode 100644 src/slam/src/slam_solver/graph_slam_solver/pose_updater.cpp create mode 100644 src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp create mode 100644 src/slam/src/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.cpp create mode 100644 src/slam/src/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.cpp diff --git a/config/car/vehicle.yaml b/config/car/vehicle.yaml index bfce6a685..ff44b1141 100644 --- a/config/car/vehicle.yaml +++ b/config/car/vehicle.yaml @@ -3,6 +3,7 @@ car: wheel_base: 1.53 track_width: 1.2 dist_cg_2_rear_axis: 0.804 + rear_axis_to_camera: 0.79 gear_ratio: 3.67 cog_height: 0.5 lift_coefficient: -0.86 diff --git a/config/slam/pacsim.yaml b/config/slam/pacsim.yaml index 04cc7c051..959ea6ce1 100644 --- a/config/slam/pacsim.yaml +++ b/config/slam/pacsim.yaml @@ -1,91 +1,49 @@ slam: - motion_model_name: "constant_velocity_turnrate" # "constant_velocity" or "constant_velocity_turnrate" or ... -> name of the motion model + motion_model_name: "constant_velocity_turnrate" # "constant_velocity" or "constant_velocity_turnrate" or "constant_acceleration_turnrate" or ... -> name of the motion model + pose_updater_name: "difference_based_ready" # "base_pose_updater" or "difference_based_ready" -> name of the pose updater object, responsible for keeping pose estimate landmark_filter_name: "consecutive_count" # "consecutive_count" or other names of filters of perception data + lidar_odometry_topic: "-" # "/fast_limo/state" or "/kiss/odometry" or "" -> topic for pose from lidar odometry + receive_lidar_odometry: false # Whether to use lidar odometry topic or not data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model - data_association_limit_distance: 30 # maximum distance to consider a cone for data association - data_association_gate: 1.23 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark + data_association_limit_distance: 70 # maximum distance to consider a cone for data association + data_association_gate: 0.9 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark new_landmark_confidence_gate: 0.6 # Minimum confidence to consider an observation as a new landmark in data asociation - observation_x_noise: 0.01 # sigma of the noise for cone positions in x - observation_y_noise: 0.01 # sigma of the noise for cone positions in y - velocity_x_noise: 0.2 # sigma of the noise for the velocity in x - velocity_y_noise: 0.2 # sigma of the noise for the velocity in y - angular_velocity_noise: 0.1 # sigma of the noise for the angular velocity - pose_x_initial_noise: 0.1 # sigma of the noise for the initial pose in x - pose_y_initial_noise: 0.1 # sigma of the noise for the initial pose in y - pose_theta_initial_noise: 0.1 # sigma of the noise for the initial pose in yaw - using_preloaded_map: true # Use preloaded map for SLAM, if true, the map will be loaded from the file only for skidpad and acceleration + observation_x_noise: 0.02 # sigma of the noise for cone positions in x (range in Graph SLAM) + observation_y_noise: 0.001 # sigma of the noise for cone positions in y (bearing in Graph SLAM) + velocity_x_noise: 0.1 # variance of the noise for the velocity in x + velocity_y_noise: 0.8 # variance of the noise for the velocity in y + imu_acceleration_x_noise: 0.005 # variance of the noise for the acceleration in x from IMU + angular_velocity_noise: 0.01 # variance of the noise for the angular velocity + pose_x_initial_noise: 0.001 # sigma of the noise for the initial pose in x + pose_y_initial_noise: 0.001 # sigma of the noise for the initial pose in y + pose_theta_initial_noise: 0.001 # sigma of the noise for the initial pose in yaw + preloaded_map_noise: 0.001 # sigma of the noise for preloaded map landmarks used for skidpad and acceleration + using_preloaded_map: false # Use preloaded map for SLAM, if true, the map will be loaded from the file only for skidpad and acceleration slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver - minimum_observation_count: 1 # Minimum number of times a new landmark must be observed to be added to the map - minimum_frequency_of_detections: 5 # Minimum frequency of detections to consider a landmark as valid - slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph - slam_optimization_mode: "sync" # "sync" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step + slam_optimization_mode: "async" # "sync" or "sync-parallel" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step and sync-parallel is sync but callbacks execute in parallel + slam_optimization_in_poses: false # If true, the optimization will be done also when poses are added, not only when observations are added slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type - preloaded_map_noise: 0.001 # sigma of the noise for preloaded map landmarks used for skidpad and acceleration + + # Only for pose_updater_name: "difference_based_ready" + slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph # Only for slam_optimization_mode: "async" - slam_optimization_period: 0.2 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added + slam_optimization_period: 0.05 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added # Only for slam_optimization_type: "isam2" - slam_isam2_relinearize_threshold: 0.005 # Threshold for relinearizing the graph in isam2 - slam_isam2_relinearize_skip: 1 # Number of steps to skip before relinearizing the graph in isam2 - slam_isam2_factorization: "QR" # QR" or "CHOLESKY" -> Factorization method for isam2 + slam_isam2_relinearize_threshold: 0.1 # Threshold for relinearizing the graph in isam2 + slam_isam2_relinearize_skip: 5 # Number of steps to skip before relinearizing the graph in isam2 + slam_isam2_factorization: "QR" # "QR" or "CHOLESKY" -> Factorization method for isam2 # Only for slam_optimization_type: "sliding_window_levenberg" - sliding_window_size: 80 # Number of poses to keep in the sliding window - -## FSG23 AutoX Track complete working - Best ever -# slam: -# motion_model_name: "constant_velocity_turnrate" # "constant_velocity" or "constant_velocity_turnrate" or ... -> name of the motion model -# data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model -# data_association_limit_distance: 30 # maximum distance to consider a cone for data association -# data_association_gate: 1.23 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark -# new_landmark_confidence_gate: 0.6 # Minimum confidence to consider an observation as a new landmark in data asociation -# observation_x_noise: 0.01 # sigma of the noise for cone positions in x -# observation_y_noise: 0.01 # sigma of the noise for cone positions in y -# velocity_x_noise: 0.2 # sigma of the noise for the velocity in x -# velocity_y_noise: 0.2 # sigma of the noise for the velocity in y -# angular_velocity_noise: 0.3 # sigma of the noise for the angular velocity -# slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver -# slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph -# slam_optimization_mode: "sync" # "sync" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step -# slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type + sliding_window_size: 50 # Number of poses to keep in the sliding window -# # Only for slam_optimization_mode: "async" -# slam_optimization_period: 0.2 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added - -# # Only for slam_optimization_type: "isam2" -# slam_isam2_relinearize_threshold: 0.005 # Threshold for relinearizing the graph in isam2 -# slam_isam2_relinearize_skip: 1 # Number of steps to skip before relinearizing the graph in isam2 -# slam_isam2_factorization: "QR" # QR" or "CHOLESKY" -> Factorization method for isam2 - -# # Only for slam_optimization_type: "sliding_window_levenberg" -# sliding_window_size: 80 # Number of poses to keep in the sliding window - - -## FSE23 Track complete working - Best ever -# slam: - # motion_model_name: "constant_velocity_turnrate" # "constant_velocity" or "constant_velocity_turnrate" or ... -> name of the motion model - # data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model - # data_association_limit_distance: 20 # maximum distance to consider a cone for data association - # data_association_gate: 1.23 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark - # new_landmark_confidence_gate: 0.6 # Minimum confidence to consider an observation as a new landmark in data asociation - # observation_x_noise: 0.01 # sigma of the noise for cone positions in x - # observation_y_noise: 0.01 # sigma of the noise for cone positions in y - # velocity_x_noise: 0.2 # sigma of the noise for the velocity in x - # velocity_y_noise: 0.2 # sigma of the noise for the velocity in y - # angular_velocity_noise: 0.2 # sigma of the noise for the angular velocity - # slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver - # slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph - # slam_optimization_mode: "sync" # "sync" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step - # slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type - - # # Only for slam_optimization_mode: "async" - # slam_optimization_period: 0.2 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added - - # # Only for slam_optimization_type: "isam2" - # slam_isam2_relinearize_threshold: 0.005 # Threshold for relinearizing the graph in isam2 - # slam_isam2_relinearize_skip: 1 # Number of steps to skip before relinearizing the graph in isam2 - # slam_isam2_factorization: "QR" # QR" or "CHOLESKY" -> Factorization method for isam2 + # Perception data filters + minimum_observation_count: 3 # Minimum number of times a new landmark must be observed to be added to the map + minimum_frequency_of_detections: 5 # Minimum frequency of detections to consider a landmark as valid - # # Only for slam_optimization_type: "sliding_window_levenberg" - # sliding_window_size: 40 # Number of poses to keep in the sliding window + # Loop closure paramteters + threshold_dist: 4.0 # Distance around origin to trigger loop closure + first_x_cones: 10 # consider a loop if you see any of these first X cones + border_width: 5 # distance to givve to start searching for loop closure again + minimum_confidence: 3 # minimum number of observations to confirm loop closure \ No newline at end of file diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index 6ca95458a..35981fd5b 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -1,34 +1,49 @@ slam: - motion_model_name: "constant_velocity" # "constant_velocity" or "constant_velocity_turnrate" or ... -> name of the motion model + motion_model_name: "constant_velocity_turnrate" # "constant_velocity" or "constant_velocity_turnrate" or "constant_acceleration_turnrate" or ... -> name of the motion model + pose_updater_name: "difference_based_ready" # "base_pose_updater" or "difference_based_ready" -> name of the pose updater object, responsible for keeping pose estimate landmark_filter_name: "consecutive_count" # "consecutive_count" or other names of filters of perception data + lidar_odometry_topic: "-" # "/fast_limo/state" or "/kiss/odometry" or "" -> topic for pose from lidar odometry + receive_lidar_odometry: false # Whether to use lidar odometry topic or not data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model data_association_limit_distance: 70 # maximum distance to consider a cone for data association - data_association_gate: 1.5 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark + data_association_gate: 0.9 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark new_landmark_confidence_gate: 0.6 # Minimum confidence to consider an observation as a new landmark in data asociation - observation_x_noise: 0.03 # sigma of the noise for cone positions in x - observation_y_noise: 0.03 # sigma of the noise for cone positions in y - velocity_x_noise: 0.03 # sigma of the noise for the velocity in x - velocity_y_noise: 0.03 # sigma of the noise for the velocity in y - angular_velocity_noise: 0.05 # sigma of the noise for the angular velocitys - pose_x_initial_noise: 0.1 # sigma of the noise for the initial pose in x - pose_y_initial_noise: 0.1 # sigma of the noise for the initial pose in y - pose_theta_initial_noise: 0.1 # sigma of the noise for the initial pose in yaw - using_preloaded_map: false # Use preloaded map for SLAM, if true, the map will be loaded from the file, only for skidpad and acceleration + observation_x_noise: 0.02 # sigma of the noise for cone positions in x (range in Graph SLAM) + observation_y_noise: 0.001 # sigma of the noise for cone positions in y (bearing in Graph SLAM) + velocity_x_noise: 0.1 # variance of the noise for the velocity in x + velocity_y_noise: 0.8 # variance of the noise for the velocity in y + imu_acceleration_x_noise: 0.005 # variance of the noise for the acceleration in x from IMU + angular_velocity_noise: 0.01 # variance of the noise for the angular velocity + pose_x_initial_noise: 0.001 # sigma of the noise for the initial pose in x + pose_y_initial_noise: 0.001 # sigma of the noise for the initial pose in y + pose_theta_initial_noise: 0.001 # sigma of the noise for the initial pose in yaw + preloaded_map_noise: 0.001 # sigma of the noise for preloaded map landmarks used for skidpad and acceleration + using_preloaded_map: false # Use preloaded map for SLAM, if true, the map will be loaded from the file only for skidpad and acceleration slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver - minimum_observation_count: 3 # Minimum number of times a new landmark must be observed to be added to the map - minimum_frequency_of_detections: 5 # Minimum frequency of the detections of a landmark to add it to the map - slam_min_pose_difference: 0.1 # Minimum distance between poses to consider adding to factor graph - slam_optimization_mode: "sync" # "sync" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step - slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type - preloaded_map_noise: 0.0 # sigma of the noise for preloaded map landmarks used for skidpad and acceleration + slam_optimization_mode: "async" # "sync" or "sync-parallel" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step and sync-parallel is sync but callbacks execute in parallel + slam_optimization_in_poses: false # If true, the optimization will be done also when poses are added, not only when observations are added + slam_optimization_type: "isam2" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type + + # Only for pose_updater_name: "difference_based_ready" + slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph # Only for slam_optimization_mode: "async" - slam_optimization_period: 0.2 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added + slam_optimization_period: 0.05 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added # Only for slam_optimization_type: "isam2" - slam_isam2_relinearize_threshold: 0.005 # Threshold for relinearizing the graph in isam2 - slam_isam2_relinearize_skip: 1 # Number of steps to skip before relinearizing the graph in isam2 - slam_isam2_factorization: "QR" # QR" or "CHOLESKY" -> Factorization method for isam2 - + slam_isam2_relinearize_threshold: 0.1 # Threshold for relinearizing the graph in isam2 + slam_isam2_relinearize_skip: 5 # Number of steps to skip before relinearizing the graph in isam2 + slam_isam2_factorization: "QR" # "QR" or "CHOLESKY" -> Factorization method for isam2 + # Only for slam_optimization_type: "sliding_window_levenberg" - sliding_window_size: 100 # Number of poses to keep in the sliding window + sliding_window_size: 50 # Number of poses to keep in the sliding window + + # Perception data filters + minimum_observation_count: 3 # Minimum number of times a new landmark must be observed to be added to the map + minimum_frequency_of_detections: 5 # Minimum frequency of detections to consider a landmark as valid + + # Loop closure paramteters + threshold_dist: 4.0 # Distance around origin to trigger loop closure + first_x_cones: 10 # consider a loop if you see any of these first X cones + border_width: 5 # distance to givve to start searching for loop closure again + minimum_confidence: 3 # minimum number of observations to confirm loop closure \ No newline at end of file diff --git a/ext/as-integration b/ext/as-integration index 8d2867cdd..7dc3311d2 160000 --- a/ext/as-integration +++ b/ext/as-integration @@ -1 +1 @@ -Subproject commit 8d2867cdd04fb38714b088797109031ba13a84dc +Subproject commit 7dc3311d2378cfb9ec2736c83888b72949d199c2 diff --git a/ext/gtsam b/ext/gtsam index 4f66a491f..c9498fe0c 160000 --- a/ext/gtsam +++ b/ext/gtsam @@ -1 +1 @@ -Subproject commit 4f66a491ffc83cf092d0d818b11dc35135521612 +Subproject commit c9498fe0c07e7750ceada4d1eb798f8f04ac9a2c diff --git a/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp b/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp index 000d1d133..2ada73d31 100644 --- a/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp +++ b/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp @@ -8,8 +8,8 @@ #include "aero_parameters.hpp" #include "common_lib/config_load/config_load.hpp" #include "steering_motor_parameters.hpp" -#include "tire_parameters.hpp" #include "steering_parameters.hpp" +#include "tire_parameters.hpp" namespace common_lib::car_parameters { struct CarParameters { @@ -17,6 +17,7 @@ struct CarParameters { double wheelbase = 1.530; double track_width = 1.2; double dist_cg_2_rear_axis = 0.804; + double rear_axis_to_camera = 0.79; double gear_ratio = 4; double cog_height = 0.5; @@ -28,8 +29,8 @@ struct CarParameters { common_lib::car_parameters::SteeringMotorParameters steering_motor_parameters; common_lib::car_parameters::SteeringParameters steering_parameters; CarParameters(); - CarParameters(double wheel_diameter, double wheelbase, double track_width, - double dist_cg_2_rear_axis, double gear_ratio); + CarParameters(double wheel_diameter, double wheelbase, double rear_axis_to_camera, + double track_width, double dist_cg_2_rear_axis, double gear_ratio); }; } // namespace common_lib::car_parameters \ No newline at end of file diff --git a/src/common_lib/include/common_lib/sensor_data/imu.hpp b/src/common_lib/include/common_lib/sensor_data/imu.hpp index 084435dcd..abdd775e3 100644 --- a/src/common_lib/include/common_lib/sensor_data/imu.hpp +++ b/src/common_lib/include/common_lib/sensor_data/imu.hpp @@ -5,14 +5,18 @@ namespace common_lib::sensor_data { struct ImuData { - float rotational_velocity; - float acceleration_x; - float acceleration_y; + double rotational_velocity; + double acceleration_x; + double acceleration_y; + double rotational_velocity_noise; + double acceleration_x_noise; + double acceleration_y_noise; rclcpp::Time timestamp_ = rclcpp::Time(0); //< Time of sensor data collection ImuData() = default; - ImuData(float rotational_velocity, float acceleration_x, float acceleration_y, - rclcpp::Time timestamp = rclcpp::Time(0)); + ImuData(double rotational_velocity, double acceleration_x, double acceleration_y, + rclcpp::Time timestamp = rclcpp::Time(0), double rotational_velocity_noise = 0.0, + double acceleration_x_noise = 0.0, double acceleration_y_noise = 0.0); }; } // namespace common_lib::sensor_data \ No newline at end of file diff --git a/src/common_lib/src/car_parameters/car_parameters.cpp b/src/common_lib/src/car_parameters/car_parameters.cpp index 2a4a3f267..88cf518dd 100644 --- a/src/common_lib/src/car_parameters/car_parameters.cpp +++ b/src/common_lib/src/car_parameters/car_parameters.cpp @@ -1,11 +1,13 @@ #include "common_lib/car_parameters/car_parameters.hpp" common_lib::car_parameters::CarParameters::CarParameters(double wheel_diameter, double wheelbase, + double rear_axis_to_camera, double track_width, double dist_cg_2_rear_axis, double gear_ratio) : wheel_diameter(wheel_diameter), wheelbase(wheelbase), + rear_axis_to_camera(rear_axis_to_camera), track_width(track_width), dist_cg_2_rear_axis(dist_cg_2_rear_axis), gear_ratio(gear_ratio) {} @@ -24,5 +26,6 @@ common_lib::car_parameters::CarParameters::CarParameters() { this->wheelbase = car_config["car"]["wheel_base"].as(); this->track_width = car_config["car"]["track_width"].as(); this->dist_cg_2_rear_axis = car_config["car"]["dist_cg_2_rear_axis"].as(); + this->rear_axis_to_camera = car_config["car"]["rear_axis_to_camera"].as(); this->gear_ratio = car_config["car"]["gear_ratio"].as(); } \ No newline at end of file diff --git a/src/common_lib/src/sensor_data/imu.cpp b/src/common_lib/src/sensor_data/imu.cpp index af6f38df8..c6153a9c5 100644 --- a/src/common_lib/src/sensor_data/imu.cpp +++ b/src/common_lib/src/sensor_data/imu.cpp @@ -2,11 +2,15 @@ namespace common_lib::sensor_data { -ImuData::ImuData(float rotational_velocity, float acceleration_x, float acceleration_y, - rclcpp::Time timestamp) +ImuData::ImuData(double rotational_velocity, double acceleration_x, double acceleration_y, + rclcpp::Time timestamp, double rotational_velocity_noise, + double acceleration_x_noise, double acceleration_y_noise) : rotational_velocity(rotational_velocity), acceleration_x(acceleration_x), acceleration_y(acceleration_y), + rotational_velocity_noise(rotational_velocity_noise), + acceleration_x_noise(acceleration_x_noise), + acceleration_y_noise(acceleration_y_noise), timestamp_(timestamp) {} } // namespace common_lib::sensor_data \ No newline at end of file diff --git a/src/motion_lib/CMakeLists.txt b/src/motion_lib/CMakeLists.txt index 655cd1845..8222e212f 100644 --- a/src/motion_lib/CMakeLists.txt +++ b/src/motion_lib/CMakeLists.txt @@ -27,12 +27,12 @@ set(IMPLEMENTATION_FILES src/vel_process_model/particle_model.cpp src/v2p_models/constant_velocity_model.cpp src/v2p_models/base_v2p_motion_model.cpp + src/s2v_model/bicycle_model.cpp + src/s2v_model/no_rear_wss_bicycle_model.cpp + src/s2v_model/no_wss_bicycle_model.cpp src/v2p_models/constant_velocity_turnrate_model.cpp - src/aero_model/default_aero_model.cpp - src/tire_model/pacejka_combined_slip.cpp - src/steering_model/parallel_front_steering.cpp - src/load_transfer_model/rigid_body_model.cpp - src/steering_motor_model/pid_steering_motor.cpp + src/v2p_models/constant_acceleration_turnrate_model.cpp + src/v2p_models/odometry_model.cpp ) add_library(${PROJECT_NAME} SHARED ${IMPLEMENTATION_FILES}) @@ -65,8 +65,10 @@ if(BUILD_TESTING) set(TESTFILES test/main.cpp + test/s2v_models/bicycle_model_test.cpp test/v2p_models/constant_velocity_model_test.cpp test/v2p_models/constant_velocity_turnrate_model_test.cpp + test/v2p_models/constant_acceleration_turnrate_model_test.cpp test/vel_process_model/particle_model_test.cpp ) diff --git a/src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp b/src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp deleted file mode 100644 index 587947d10..000000000 --- a/src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include - -#include "common_lib/car_parameters/car_parameters.hpp" - -class AeroModel { -protected: - std::shared_ptr car_parameters_; - -public: - AeroModel(const common_lib::car_parameters::CarParameters& car_parameters) - : car_parameters_( - std::make_shared(car_parameters)) {} - /** - * @brief Calculate the aero forces - * - * @param velocity Velocity of the car in m/s in the car's body frame [vx, vy, angular_velocity] - * @return Eigen::Vector3d Aero forces in the car's body frame [Fx, Fy, Fz] in Newtons - */ - virtual Eigen::Vector3d aero_forces(const Eigen::Vector3d& velocity) const = 0; -}; diff --git a/src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp b/src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp deleted file mode 100644 index 7c6e64e5a..000000000 --- a/src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include "base_aero_model.hpp" - -class DefaultAeroModel : public AeroModel { -public: - Eigen::Vector3d aero_forces(const Eigen::Vector3d& velocity) const override; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/aero_model/map.hpp b/src/motion_lib/include/motion_lib/aero_model/map.hpp deleted file mode 100644 index 455b048f0..000000000 --- a/src/motion_lib/include/motion_lib/aero_model/map.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "default_aero_model.hpp" - -/* - * Map of aero models, with the key being the name of the aero model and the value being a lambda - * function that returns a shared pointer to the corresponding aero model - */ -const std::map< - std::string, - std::function(const common_lib::car_parameters::CarParameters&)>, - std::less<>> - aero_models_map = { - {"default_aero", - [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { - return std::make_shared(params); - }}, -}; diff --git a/src/motion_lib/include/motion_lib/car_parameters.hpp b/src/motion_lib/include/motion_lib/car_parameters.hpp new file mode 100644 index 000000000..9589f4a6a --- /dev/null +++ b/src/motion_lib/include/motion_lib/car_parameters.hpp @@ -0,0 +1,7 @@ +#pragma once + +constexpr double WHEEL_DIAMETER = 0.5; +constexpr double WHEELBASE = 1.530; +constexpr double REAR_AXIS_TO_CAMERA = 0.79; +constexpr double AXIS_LENGTH = 1.2; +constexpr double DIST_CG_2_REAR_AXIS = 0.9822932352409; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp b/src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp deleted file mode 100644 index 209e859a7..000000000 --- a/src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include -#include - -#include "common_lib/car_parameters/car_parameters.hpp" - -/** - * @brief Base class for models that compute load transfer on a vehicle. - * - */ -class LoadTransferModel { -protected: - std::shared_ptr car_parameters_; - -public: - LoadTransferModel(const common_lib::car_parameters::CarParameters& car_parameters) - : car_parameters_( - std::make_shared(car_parameters)) {} - - /** - * @brief Computes loads on the tires based on the dynamic state of the vehicle. - * - * @param dynamic_state Contains the relevant dynamic state that affects the load transfer, can - * include accelerations, euler angles, velocities, etc. - * @return Eigen::Vector4d a vector containing the loads on the four tires in Newtons, in the - * order: FL, FR, RL, RR. - */ - virtual Eigen::Vector4d compute_loads(const Eigen::VectorXd& dynamic_state) const = 0; -}; diff --git a/src/motion_lib/include/motion_lib/load_transfer_model/map.hpp b/src/motion_lib/include/motion_lib/load_transfer_model/map.hpp deleted file mode 100644 index 437f458b2..000000000 --- a/src/motion_lib/include/motion_lib/load_transfer_model/map.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "motion_lib/load_transfer_model/rigid_body_model.hpp" - -/* - * Map of load transfer models, with the key being the name of the model and the value being a - * lambda function that returns a shared pointer to the corresponding load transfer model. - */ -const std::map( - const common_lib::car_parameters::CarParameters&)>, - std::less<>> - load_transfer_models_map = { - {"rigid_body", - [](const common_lib::car_parameters::CarParameters& params) - -> std::shared_ptr { - return std::make_shared(params); - }}, -}; diff --git a/src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp b/src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp deleted file mode 100644 index 53f87430b..000000000 --- a/src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#include "motion_lib/load_transfer_model/base_load_transfer_model.hpp" - -class RigidBodyLoadTransferModel : public LoadTransferModel { -public: - RigidBodyLoadTransferModel(const common_lib::car_parameters::CarParameters& car_parameters) - : LoadTransferModel(car_parameters) {} - - /** - * @brief Computes loads on the tires based on the dynamic state of the vehicle. - * - * @param dynamic_state Receives only the lateral and longitudinal accelerations - * @return Eigen::Vector4d a vector containing the loads on the four tires in Newtons, in the - * order: FL, FR, RL, RR. - */ - Eigen::Vector4d compute_loads(const Eigen::VectorXd& dynamic_state) const override; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/s2v_model/bicycle_model.hpp b/src/motion_lib/include/motion_lib/s2v_model/bicycle_model.hpp new file mode 100644 index 000000000..ef52e3694 --- /dev/null +++ b/src/motion_lib/include/motion_lib/s2v_model/bicycle_model.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include + +#include "common_lib/maths/transformations.hpp" +#include "motion_lib/s2v_model/s2v_model.hpp" + +class BicycleModel : public S2VModel { + common_lib::car_parameters::CarParameters car_parameters_; + +public: + BicycleModel(common_lib::car_parameters::CarParameters car_parameters) + : car_parameters_(car_parameters) {} + /** + * @brief assumes a bicycle model and a set of parameters about the vehicle to calculate the + * velocities of the center of mass given the velocities of the wheels + * + * @param rl_rpm rear left wheel velocity in rpm + * @param fl_rpm front left wheel velocity in rpm + * @param rr_rpm rear right wheel velocity in rpm + * @param fr_rpm front right wheel velocity in rpm + * @param steering_angle steering angle in radians + * @return std::pair translational and rotational velocities of the center of mass + * in m/s and rad/s respectively + */ + std::pair wheels_velocities_to_cg(double rl_rpm, [[maybe_unused]] double fl_rpm, + double rr_rpm, [[maybe_unused]] double fr_rpm, + double steering_angle) override; + + /** + * @brief Calculates the position of the rear axis given the position of the center of mass, the + * the orientation and the distance between the center of mass and the rear axis + * + * @param cg position of the center of mass in meters + * @param orientation orientation of the vehicle relative to the world frame in radians + * @param dist_cg_2_rear_axis disntance between the center of mass and the rear axis in meters + * @return common_lib::structures::Position + */ + common_lib::structures::Position rear_axis_position(const common_lib::structures::Position& cg, + double orientation, + double dist_cg_2_rear_axis) override; + /** + * @brief Assumes a bicycle model and a set of parameters about the vehicle to calculate the + * velocities of the wheels given the velocities of the center of mass + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::VectorXd rpms of the wheels, the steering angle, and motor rpms {fl_rpm, fr_rpm, + * rl_rpm, rr_rpm, steering_angle, motor_rpm} + */ + Eigen::VectorXd cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) override; + + /** + * @brief Calculates the jacobian of the function cg_velocity_to_wheels with respect to the center + * of mass velocities + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::MatrixXd jacobian matrix of dimension 6x3 + */ + Eigen::MatrixXd jacobian_cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/s2v_model/map.hpp b/src/motion_lib/include/motion_lib/s2v_model/map.hpp new file mode 100644 index 000000000..52aa2fc22 --- /dev/null +++ b/src/motion_lib/include/motion_lib/s2v_model/map.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include + +#include "motion_lib/s2v_model/bicycle_model.hpp" +#include "motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp" +#include "motion_lib/s2v_model/no_wss_bicycle_model.hpp" + +/* + * Map of slam_solvers, with the key being the type of the slam_solver and the value being a lambda + * function that returns a shared pointer to the corresponding slam_solver + */ +const std::map< + std::string, + std::function(const common_lib::car_parameters::CarParameters&)>, + std::less<>> + s2v_models_map = { + {"bicycle_model", + [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, + {"no_rear_wss_bicycle_model", + [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, + {"no_wss_bicycle_model", + [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/motion_lib/include/motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp b/src/motion_lib/include/motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp new file mode 100644 index 000000000..02f1b2c46 --- /dev/null +++ b/src/motion_lib/include/motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include + +#include "common_lib/maths/transformations.hpp" +#include "motion_lib/s2v_model/s2v_model.hpp" + +class NoRearWSSBicycleModel : public S2VModel { + common_lib::car_parameters::CarParameters car_parameters_; + +public: + NoRearWSSBicycleModel(common_lib::car_parameters::CarParameters car_parameters) + : car_parameters_(car_parameters) {} + /** + * @brief assumes a bicycle model and a set of parameters about the vehicle to calculate the + * velocities of the center of mass given the velocities of the wheels + * + * @param rl_rpm rear left wheel velocity in rpm + * @param fl_rpm front left wheel velocity in rpm + * @param steering_angle steering angle in radians + * @return std::pair translational and rotational velocities of the center of mass + * in m/s and rad/s respectively + */ + std::pair wheels_velocities_to_cg(double rl_rpm, [[maybe_unused]] double fl_rpm, + double rr_rpm, [[maybe_unused]] double fr_rpm, + double steering_angle) override; + + /** + * @brief Calculates the position of the rear axis given the position of the center of mass, the + * the orientation and the distance between the center of mass and the rear axis + * + * @param cg position of the center of mass in meters + * @param orientation orientation of the vehicle relative to the world frame in radians + * @param dist_cg_2_rear_axis disntance between the center of mass and the rear axis in meters + * @return common_lib::structures::Position + */ + common_lib::structures::Position rear_axis_position(const common_lib::structures::Position& cg, + double orientation, + double dist_cg_2_rear_axis) override; + /** + * @brief Assumes a bicycle model and a set of parameters about the vehicle to calculate the + * velocities of the wheels given the velocities of the center of mass + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::VectorXd rpms of the wheels, the steering angle, and motor rpms {fl_rpm, fr_rpm, + * steering_angle, motor_rpm} + */ + Eigen::VectorXd cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) override; + + /** + * @brief Calculates the jacobian of the function cg_velocity_to_wheels with respect to the center + * of mass velocities + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::MatrixXd jacobian matrix of dimension 6x3 + */ + Eigen::MatrixXd jacobian_cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/s2v_model/no_wss_bicycle_model.hpp b/src/motion_lib/include/motion_lib/s2v_model/no_wss_bicycle_model.hpp new file mode 100644 index 000000000..8ca8e89d3 --- /dev/null +++ b/src/motion_lib/include/motion_lib/s2v_model/no_wss_bicycle_model.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include + +#include "common_lib/maths/transformations.hpp" +#include "motion_lib/s2v_model/s2v_model.hpp" + +class NoWSSBicycleModel : public S2VModel { + common_lib::car_parameters::CarParameters car_parameters_; + +public: + NoWSSBicycleModel(common_lib::car_parameters::CarParameters car_parameters) + : car_parameters_(car_parameters) {} + /** + * @brief assumes a bicycle model and a set of parameters about the vehicle to calculate the + * velocities of the center of mass given the velocities of the wheels + * + * @param rl_rpm rear left wheel velocity in rpm + * @param fl_rpm front left wheel velocity in rpm + * @param steering_angle steering angle in radians + * @return std::pair translational and rotational velocities of the center of mass + * in m/s and rad/s respectively + */ + std::pair wheels_velocities_to_cg(double rl_rpm, [[maybe_unused]] double fl_rpm, + double rr_rpm, [[maybe_unused]] double fr_rpm, + double steering_angle) override; + + /** + * @brief Calculates the position of the rear axis given the position of the center of mass, the + * the orientation and the distance between the center of mass and the rear axis + * + * @param cg position of the center of mass in meters + * @param orientation orientation of the vehicle relative to the world frame in radians + * @param dist_cg_2_rear_axis disntance between the center of mass and the rear axis in meters + * @return common_lib::structures::Position + */ + common_lib::structures::Position rear_axis_position(const common_lib::structures::Position& cg, + double orientation, + double dist_cg_2_rear_axis) override; + /** + * @brief Assumes a bicycle model and a set of parameters about the vehicle to calculate the + * velocities of the wheels given the velocities of the center of mass + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::VectorXd rpms of the wheels, the steering angle, and motor rpms {fl_rpm, fr_rpm, + * steering_angle, motor_rpm} + */ + Eigen::VectorXd cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) override; + + /** + * @brief Calculates the jacobian of the function cg_velocity_to_wheels with respect to the center + * of mass velocities + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::MatrixXd jacobian matrix of dimension 6x3 + */ + Eigen::MatrixXd jacobian_cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/s2v_model/s2v_model.hpp b/src/motion_lib/include/motion_lib/s2v_model/s2v_model.hpp new file mode 100644 index 000000000..bf51c6947 --- /dev/null +++ b/src/motion_lib/include/motion_lib/s2v_model/s2v_model.hpp @@ -0,0 +1,69 @@ +#pragma once +#include +#include + +#include "common_lib/car_parameters/car_parameters.hpp" +#include "common_lib/structures/position.hpp" +/** + * @brief Interface for models that convert data from sensor reference frame to vehicle reference + * frame and vice versa + * + */ +class S2VModel { +public: + /** + * @brief Calculates the translational and rotational velocities of the vehicle from wheel + * rotations and steering angle + * + * @param rl_rpm rear left wheel rpms + * @param fl_rpm front left wheel rpms + * @param rr_rpm rear right wheel rpms + * @param fr_rpm front right wheel rpms + * @param steering_angle steering angle in radians + * + * @return std::pair translational velocity, rotational velocity + */ + virtual std::pair wheels_velocities_to_cg(double rl_rpm, + [[maybe_unused]] double fl_rpm, + double rr_rpm, + [[maybe_unused]] double fr_rpm, + double steering_angle) = 0; + + /** + * @brief Calculate rear axis coordinates + * + * @param cg center of gravity position + * @param orientation orientation of the vehicle in radians relative to the world frame (ccw) + * @param dist_cg_2_rear_axis distance between the center of gravity and the rear axis + * + * @return Point + */ + virtual common_lib::structures::Position rear_axis_position( + const common_lib::structures::Position& cg, double orientation, + double dist_cg_2_rear_axis) = 0; + + /** + * @brief Estimate wheel and motor velocities, as well as steering angle from the velocities of + * the center of gravity + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::VectorXd vector of wheel speeds, steering angle and motor speed + * {fl_rpm, fr_rpm, rl_rpm, rr_rpm, steering_angle, motor_rpm} + */ + virtual Eigen::VectorXd cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) = 0; + + /** + * @brief jacobian of the function cg_velocity_to_wheels with respect to the velocities of the + * center of gravity + * + * @details Each entry at row i and column j of the resulting matrix is the partial derivative of + * the i-th element of the output of function cg_velocity_to_wheels with respect to the j-th + * element of the vector cg_velocities + * + * @param cg_velocities vector of velocities on the Center of Gravity {velocity_x, velocity_y, + * rotational_velocity} in m/s and rad/s respectively + * @return Eigen::MatrixXd jacobian matrix of the function cg_velocity_to_wheels (dimensions: 6x3) + */ + virtual Eigen::MatrixXd jacobian_cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) = 0; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp b/src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp deleted file mode 100644 index d846bc1aa..000000000 --- a/src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include - -#include "common_lib/car_parameters/car_parameters.hpp" - -class SteeringModel { -protected: - std::shared_ptr car_parameters_; - -public: - SteeringModel(const common_lib::car_parameters::CarParameters& car_parameters) - : car_parameters_( - std::make_shared(car_parameters)) {} - /** - * @brief Calculate the steering angles on each wheel based on the steering wheel angle. - * - * @param steering_wheel_angle The angle of the steering wheel in radians. - * @return Eigen::Vector4d A vector containing the steering angles for each wheel: - * [front left, front right, rear left, rear right] in radians. - */ - virtual Eigen::Vector4d calculate_steering_angles(double steering_wheel_angle) const = 0; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_model/map.hpp b/src/motion_lib/include/motion_lib/steering_model/map.hpp deleted file mode 100644 index cbd98b0c3..000000000 --- a/src/motion_lib/include/motion_lib/steering_model/map.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "parallel_front_steering.hpp" - -/* - * Map of steering models, with the key being the name of the steering model and the value being a - * lambda function that returns a shared pointer to the corresponding steering model - */ -const std::map< - std::string, - std::function(const common_lib::car_parameters::CarParameters&)>, - std::less<>> - steering_models_map = { - {"parallel_front_steering", - [](const common_lib::car_parameters::CarParameters& params) - -> std::shared_ptr { - return std::make_shared(params); - }}, -}; diff --git a/src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp b/src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp deleted file mode 100644 index 86e995ab1..000000000 --- a/src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "motion_lib/steering_model/base_steering_model.hpp" -/** - * @brief Steering model that assumes parallel front steering, meaning that front wheels turn as - * much as the steering wheel angle, and rear wheels do not steer. - * - */ -class ParallelFrontSteering : public SteeringModel { -public: - explicit ParallelFrontSteering(common_lib::car_parameters::CarParameters car_parameters) - : SteeringModel(car_parameters) {} - Eigen::Vector4d calculate_steering_angles(double steering_wheel_angle) const override; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp b/src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp deleted file mode 100644 index ebe377c4d..000000000 --- a/src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include -#include - -#include "common_lib/car_parameters/car_parameters.hpp" - -class SteeringMotorModel { -protected: - std::shared_ptr car_parameters_; - -public: - SteeringMotorModel(const common_lib::car_parameters::CarParameters& car_parameters) - : car_parameters_( - std::make_shared(car_parameters)) {} - - /** - * @brief Computes the change in steering angle based. - * - * @param current_steering current steering angle in radians - * @param steering_goal desired steering angle in radians - * @return double change in steering angle in radians - */ - virtual double compute_steering_rate(double current_steering, double steering_goal) = 0; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_motor_model/map.hpp b/src/motion_lib/include/motion_lib/steering_motor_model/map.hpp deleted file mode 100644 index af7d9cba7..000000000 --- a/src/motion_lib/include/motion_lib/steering_motor_model/map.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "motion_lib/steering_motor_model/pid_steering_motor.hpp" - -/* - * Map of steering motor models, with the key being the name of the steering motor model and the - * value being a lambda function that returns a shared pointer to the corresponding steering motor - * model - */ -const std::map( - const common_lib::car_parameters::CarParameters&)>, - std::less<>> - steering_motor_models_map = { - {"pid", - [](const common_lib::car_parameters::CarParameters& params) - -> std::shared_ptr { - return std::make_shared(params); - }}, -}; diff --git a/src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp b/src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp deleted file mode 100644 index ee6a6eceb..000000000 --- a/src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include - -#include "motion_lib/steering_motor_model/base_steering_motor_model.hpp" - -class PIDSteeringMotor : public SteeringMotorModel { -private: - double integral_ = 0.0; // Integral term accumulator - double previous_error_ = 0.0; // Previous error for derivative calculation - rclcpp::Time last_update = rclcpp::Time(0, 0); - -public: - PIDSteeringMotor(const common_lib::car_parameters::CarParameters& car_parameters) - : SteeringMotorModel(car_parameters) {} - - /** - * @brief Computes the change in steering angle based on PID control. - * - * @param current_steering current steering angle in radians - * @param steering_goal desired steering angle in radians - * @return double change in steering angle in radians - */ - double compute_steering_rate(double current_steering, double steering_goal) override; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp b/src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp deleted file mode 100644 index cb94d9056..000000000 --- a/src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include "common_lib/car_parameters/car_parameters.hpp" -/** - * @brief Class used to model tires. Currently used to model tire forces based on slip. - * - */ -class TireModel { -protected: - std::shared_ptr car_parameters_; - -public: - TireModel(const common_lib::car_parameters::CarParameters& car_parameters) - : car_parameters_( - std::make_shared(car_parameters)) {} - /** - * @brief Calculate the forces acting in a tire based on the tire characteristics and dynamic - * state. - * - * @param slip_angle Slip angle of the tire in radians - * @param slip_ratio Slip ratio of the tire - * @param load Load on the tire in Newtons - * @return std::pair Longitudinal and lateral forces - */ - virtual std::pair tire_forces(double slip_angle, double slip_ratio, - double vertical_load) const = 0; -}; diff --git a/src/motion_lib/include/motion_lib/tire_model/map.hpp b/src/motion_lib/include/motion_lib/tire_model/map.hpp deleted file mode 100644 index fe64f73c2..000000000 --- a/src/motion_lib/include/motion_lib/tire_model/map.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "pacejka_combined_slip.hpp" - -/* - * Map of tire models, with the key being the name of the tire model and the value being a lambda - * function that returns a shared pointer to the corresponding tire model - */ -const std::map< - std::string, - std::function(const common_lib::car_parameters::CarParameters&)>, - std::less<>> - tire_models_map = { - {"pacejka_combined_slip", - [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { - return std::make_shared(params); - }}, -}; diff --git a/src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp b/src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp deleted file mode 100644 index 52c848441..000000000 --- a/src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -#include "base_tire_model.hpp" - -class PacejkaCombinedSlip : public TireModel { -public: - std::pair tire_forces(double slip_angle, double slip_ratio, - double vertical_load) const override; -}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/v2p_models/base_v2p_motion_model.hpp b/src/motion_lib/include/motion_lib/v2p_models/base_v2p_motion_model.hpp index 9a9cb8e14..8d221bf06 100644 --- a/src/motion_lib/include/motion_lib/v2p_models/base_v2p_motion_model.hpp +++ b/src/motion_lib/include/motion_lib/v2p_models/base_v2p_motion_model.hpp @@ -6,76 +6,58 @@ * @brief Base class for the 'vehicle to pose' (more in interal background review) motion models * * @details This class defines the interface for the motion models that predict the pose of the - * robot given the velocities + * robot given the motion data */ class V2PMotionModel { - Eigen::Vector3d - _base_process_noise_; //< Equivalent to the diagonal of the process noise covariance matrix - public: - explicit V2PMotionModel(); - - /** - * @brief Construct a new ConstantVelocityModel object with a base process noise - * - * @param base_process_noise standard non variating noise if used - */ - explicit V2PMotionModel(const Eigen::Vector3d base_process_noise); - virtual ~V2PMotionModel() = default; /** - * @brief Predict the pose of the robot given the velocities + * @brief Predict the pose of the robot given the motion data * * @param previous_pose - * @param velocities (vx, vy, omega) + * @param motion_data (vx, vy, omega, ax) * @param delta_t * @return Eigen::Vector3d */ virtual Eigen::Vector3d get_next_pose(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, const double delta_t); + const Eigen::VectorXd &motion_data, const double delta_t); /** * @brief Gives the increments to the pose instead of the next pose * * @param previous_pose - * @param velocities (vx, vy, omega) + * @param motion_data (vx, vy, omega, ax) * @param delta_t * @return Eigen::Vector3d */ virtual Eigen::Vector3d get_pose_difference(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) = 0; /** * @brief Get the Jacobian matrix of the motion model in relation to the pose (state) + * @details This is used to multiplty by the previous covariance to get the matrix, propagating + * the previous pose error to the current pose error (variance) * @param previous_pose - * @param velocities + * @param motion_data (vx, vy, omega, ax) * @param delta_t * @return Eigen::Matrix3d */ virtual Eigen::Matrix3d get_jacobian_pose(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) = 0; /** - * @brief Get the Jacobian matrix of the motion model in relation to velocities (commands) + * @brief Get the Jacobian matrix of the motion model in relation to motion data (commands) + * @details This is used to multiplty by the motion data noise to get the matrix to be summed to + * the covariance * @param previous_pose - * @param velocities + * @param motion_data (vx, vy, omega, ax) * @param delta_t * @return Eigen::Matrix3d */ - virtual Eigen::Matrix3d get_jacobian_velocities( - const Eigen::Vector3d &previous_pose, [[maybe_unused]] const Eigen::Vector3d &velocities, + virtual Eigen::MatrixXd get_jacobian_motion_data( + const Eigen::Vector3d &previous_pose, [[maybe_unused]] const Eigen::VectorXd &motion_data, const double delta_t) = 0; - - /** - * @brief Get the process noise matrix - */ - Eigen::Matrix3d get_base_process_noise_matrix(); - - /** - * @brief Get the process noise vector - */ - Eigen::Vector3d get_base_process_noise(); }; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp b/src/motion_lib/include/motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp new file mode 100644 index 000000000..c988b3071 --- /dev/null +++ b/src/motion_lib/include/motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include "motion_lib/v2p_models/base_v2p_motion_model.hpp" + +/** + * @brief Motion model that predicts the pose of the robot given the velocities + * with a constant acceleration and turnrate model + */ +class ConstantAccelerationTurnrateModel : public V2PMotionModel { +public: + /** + * @brief Gives the increments to the pose instead of the next pose + * + * @param previous_pose + * @param motion_data (vx, vy, omega, ax) + * @param delta_t + * @return Eigen::Vector3d + */ + Eigen::Vector3d get_pose_difference(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + const double delta_t) override; + + /** + * @brief Get the Jacobian matrix of the motion model in relation to the pose (state) + * @details This is used to multiplty by the previous covariance to get the matrix, propagating + * the previous pose error to the current pose error (variance) + * @param previous_pose + * @param motion_data (vx, vy, omega, ax) + * @param delta_t + * @return Eigen::Matrix3d + */ + Eigen::Matrix3d get_jacobian_pose(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + const double delta_t) override; + + /** + * @brief Get the Jacobian matrix of the motion model in relation to motion data (commands) + * @details This is used to multiplty by the motion data noise to get the matrix to be summed to + * the covariance + * @param previous_pose + * @param motion_data (vx, vy, omega, ax) + * @param delta_t + * @return Eigen::Matrix3d + */ + Eigen::MatrixXd get_jacobian_motion_data(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + const double delta_t) override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_model.hpp b/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_model.hpp index 2288a8af8..52b282238 100644 --- a/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_model.hpp +++ b/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_model.hpp @@ -10,46 +10,41 @@ */ class ConstantVelocityModel : public V2PMotionModel { public: - explicit ConstantVelocityModel(); - - /** - * @brief Construct a new ConstantVelocityModel object with a base process noise - * - * @param base_process_noise standard non variating noise if used - */ - explicit ConstantVelocityModel(const Eigen::Vector3d base_process_noise); - /** * @brief Gives the increments to the pose instead of the next pose * * @param previous_pose - * @param velocities (vx, vy, omega) + * @param motion_data (vx, vy, omega) * @param delta_t * @return Eigen::Vector3d */ Eigen::Vector3d get_pose_difference(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) override; /** - * @brief Get the Jacobian matrix of the motion model in relation to velocities (commands) + * @brief Get the Jacobian matrix of the motion model in relation to the pose (state) + * @details This is used to multiplty by the previous covariance to get the matrix, propagating + * the previous pose error to the current pose error (variance) * @param previous_pose - * @param velocities + * @param motion_data * @param delta_t * @return Eigen::Matrix3d */ Eigen::Matrix3d get_jacobian_pose(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) override; /** - * @brief Get the Jacobian matrix of the motion model in relation to velocities (commands) + * @brief Get the Jacobian matrix of the motion model in relation to motion data (commands) + * @details This is used to multiplty by the motion data noise to get the matrix to be summed to + * the covariance * @param previous_pose - * @param velocities + * @param motion_data * @param delta_t * @return Eigen::Matrix3d */ - Eigen::Matrix3d get_jacobian_velocities(const Eigen::Vector3d &previous_pose, - [[maybe_unused]] const Eigen::Vector3d &velocities, - const double delta_t) override; + Eigen::MatrixXd get_jacobian_motion_data(const Eigen::Vector3d &previous_pose, + [[maybe_unused]] const Eigen::VectorXd &motion_data, + const double delta_t) override; }; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_turnrate_model.hpp b/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_turnrate_model.hpp index 193a372bb..9d4a40642 100644 --- a/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_turnrate_model.hpp +++ b/src/motion_lib/include/motion_lib/v2p_models/constant_velocity_turnrate_model.hpp @@ -10,46 +10,41 @@ */ class ConstantVelocityTurnrateModel : public V2PMotionModel { public: - explicit ConstantVelocityTurnrateModel(); - - /** - * @brief Construct a new ConstantVelocityTurnrateModel object with a base process noise - * - * @param base_process_noise standard non variating noise if used - */ - explicit ConstantVelocityTurnrateModel(const Eigen::Vector3d base_process_noise); - /** * @brief Gives the increments to the pose instead of the next pose * * @param previous_pose - * @param velocities (vx, vy, omega) + * @param motion_data (vx, vy, omega) * @param delta_t * @return Eigen::Vector3d */ Eigen::Vector3d get_pose_difference(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) override; /** - * @brief Get the Jacobian matrix of the motion model in relation to velocities (commands) + * @brief Get the Jacobian matrix of the motion model in relation to the pose (state) + * @details This is used to multiplty by the previous covariance to get the matrix, propagating + * the previous pose error to the current pose error (variance) * @param previous_pose - * @param velocities + * @param motion_data * @param delta_t * @return Eigen::Matrix3d */ Eigen::Matrix3d get_jacobian_pose(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) override; /** - * @brief Get the Jacobian matrix of the motion model in relation to velocities (commands) + * @brief Get the Jacobian matrix of the motion model in relation to motion data (commands) + * @details This is used to multiplty by the motion data noise to get the matrix to be summed to + * the covariance * @param previous_pose - * @param velocities + * @param motion_data * @param delta_t * @return Eigen::Matrix3d */ - Eigen::Matrix3d get_jacobian_velocities(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, - const double delta_t) override; + Eigen::MatrixXd get_jacobian_motion_data(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + const double delta_t) override; }; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/v2p_models/map.hpp b/src/motion_lib/include/motion_lib/v2p_models/map.hpp index c89ed5c25..4054e1474 100644 --- a/src/motion_lib/include/motion_lib/v2p_models/map.hpp +++ b/src/motion_lib/include/motion_lib/v2p_models/map.hpp @@ -5,12 +5,13 @@ #include #include "motion_lib/v2p_models/base_v2p_motion_model.hpp" +#include "motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp" #include "motion_lib/v2p_models/constant_velocity_model.hpp" #include "motion_lib/v2p_models/constant_velocity_turnrate_model.hpp" /* - * Map of slam_solvers, with the key being the type of the slam_solver and the value being a lambda - * function that returns a shared pointer to the corresponding slam_solver + * Map of motion models, with the key being the type of the motion model and the value being a + * lambda function that returns a shared pointer to the corresponding motion model */ const std::map()>, std::less<>> v2p_models_map = { @@ -22,4 +23,8 @@ const std::map()>, st []() -> std::shared_ptr { return std::make_shared(); }}, + {"constant_acceleration_turnrate", + []() -> std::shared_ptr { + return std::make_shared(); + }}, }; diff --git a/src/motion_lib/include/motion_lib/v2p_models/odometry_model.hpp b/src/motion_lib/include/motion_lib/v2p_models/odometry_model.hpp new file mode 100644 index 000000000..6155f52e9 --- /dev/null +++ b/src/motion_lib/include/motion_lib/v2p_models/odometry_model.hpp @@ -0,0 +1,52 @@ +#pragma once + +#pragma once + +#include + +#include "motion_lib/v2p_models/base_v2p_motion_model.hpp" + +/** + * @brief Motion model to apply to odometry sources which already output pose + * + * @details This model is used so that odometry sources also have a motion model and code remains + * the same for every solution + */ +class OdometryModel : public V2PMotionModel { +public: + /** + * @brief Gives the increments to the pose instead of the next pose + * + * @param previous_pose + * @param motion_data (Deltax, Deltay, Delta_theta) + * @param delta_t + * @return Eigen::Vector3d + */ + Eigen::Vector3d get_pose_difference(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + const double delta_t) override; + + /** + * @brief Get the Jacobian matrix of the motion model in relation to motion_data (commands) + * @param previous_pose + * @param motion_data (Deltax, Deltay, Delta_theta) + * @param delta_t + * @return Eigen::Matrix3d + */ + Eigen::Matrix3d get_jacobian_pose(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + const double delta_t) override; + + /** + * @brief Get the Jacobian matrix of the motion model in relation to motion data (commands) + * @details This is used to multiplty by the motion data noise to get the matrix to be summed to + * the covariance + * @param previous_pose + * @param motion_data (Deltax, Deltay, Delta_theta) + * @param delta_t + * @return Eigen::Matrix3d + */ + Eigen::MatrixXd get_jacobian_motion_data(const Eigen::Vector3d &previous_pose, + [[maybe_unused]] const Eigen::VectorXd &motion_data, + const double delta_t) override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/vel_process_model/base_vel_process_model.hpp b/src/motion_lib/include/motion_lib/vel_process_model/base_vel_process_model.hpp index 1cae72438..1aeae4667 100644 --- a/src/motion_lib/include/motion_lib/vel_process_model/base_vel_process_model.hpp +++ b/src/motion_lib/include/motion_lib/vel_process_model/base_vel_process_model.hpp @@ -32,4 +32,15 @@ class BaseVelocityProcessModel { virtual Eigen::Matrix3d get_jacobian_velocities( const Eigen::Vector3d& previous_velocities, [[maybe_unused]] const Eigen::Vector3d& accelerations, const double time_interval) = 0; + + /** + * @brief Returns the Jacobian of the velocity process model with respect to the accelerations. + * @param previous_velocities The velocities at the previous time step. + * @param accelerations The accelerations applied during the time interval. + * @param time_interval The time interval over which the accelerations are applied. + * @return The Jacobian matrix of the velocity process model with respect to the accelerations + */ + virtual Eigen::Matrix3d get_jacobian_sensor_data( + [[maybe_unused]] const Eigen::Vector3d& previous_velocities, + [[maybe_unused]] const Eigen::Vector3d& accelerations, const double time_interval) = 0; }; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/vel_process_model/kinematic_bicycle.hpp b/src/motion_lib/include/motion_lib/vel_process_model/kinematic_bicycle.hpp deleted file mode 100755 index e69de29bb..000000000 diff --git a/src/motion_lib/include/motion_lib/vel_process_model/particle_model.hpp b/src/motion_lib/include/motion_lib/vel_process_model/particle_model.hpp index c0e64ee80..4a8d60d1e 100644 --- a/src/motion_lib/include/motion_lib/vel_process_model/particle_model.hpp +++ b/src/motion_lib/include/motion_lib/vel_process_model/particle_model.hpp @@ -25,4 +25,15 @@ class CAParticleModel : public BaseVelocityProcessModel { Eigen::Matrix3d get_jacobian_velocities(const Eigen::Vector3d& previous_velocities, [[maybe_unused]] const Eigen::Vector3d& imu_data, const double time_interval) override; + + /** + * @brief Returns the Jacobian of the velocity process model with respect to the accelerations. + * @param previous_velocities The velocities at the previous time step. + * @param accelerations The accelerations applied during the time interval. + * @param time_interval The time interval over which the accelerations are applied. + * @return The Jacobian matrix of the velocity process model with respect to the accelerations + */ + Eigen::Matrix3d get_jacobian_sensor_data( + [[maybe_unused]] const Eigen::Vector3d& previous_velocities, + [[maybe_unused]] const Eigen::Vector3d& accelerations, const double time_interval) override; }; diff --git a/src/motion_lib/src/aero_model/default_aero_model.cpp b/src/motion_lib/src/aero_model/default_aero_model.cpp deleted file mode 100644 index 6cd23e21b..000000000 --- a/src/motion_lib/src/aero_model/default_aero_model.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "motion_lib/aero_model/default_aero_model.hpp" - -Eigen::Vector3d DefaultAeroModel::aero_forces(const Eigen::Vector3d& velocity) const { - const double vx = velocity[0]; - const double vy = velocity[1]; - - const double air_density = 1.225; // [kg/m^3] TODO (maybe): make this a parameter - const double frontal_area = this->car_parameters_->aero_parameters.frontal_area; // [m^2] - const double drag_coefficient = this->car_parameters_->aero_parameters.drag_coefficient; - const double side_force_coefficient = - this->car_parameters_->aero_parameters.aero_side_force_coefficient; - const double lift_coefficient = this->car_parameters_->aero_parameters.lift_coefficient; - - // Drag force (opposes vx) - const double Fx = -0.5 * air_density * frontal_area * drag_coefficient * vx * std::abs(vx); - - // Side force (opposes vy) - const double Fy = -0.5 * air_density * frontal_area * side_force_coefficient * vy * std::abs(vy); - - // Downforce/Lift - const double Fz = -0.5 * air_density * frontal_area * lift_coefficient * vx * vx; - - return Eigen::Vector3d(Fx, Fy, Fz); -} \ No newline at end of file diff --git a/src/motion_lib/src/load_transfer_model/rigid_body_model.cpp b/src/motion_lib/src/load_transfer_model/rigid_body_model.cpp deleted file mode 100644 index 00b047304..000000000 --- a/src/motion_lib/src/load_transfer_model/rigid_body_model.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "motion_lib/load_transfer_model/rigid_body_model.hpp" - -Eigen::Vector4d RigidBodyLoadTransferModel::compute_loads( - const Eigen::VectorXd& dynamic_state) const { - if (dynamic_state.size() != 2) { - throw std::invalid_argument( - "Dynamic state for Rigid Body Load Transfer Model must contain exactly two elements: " - "lateral and longitudinal accelerations."); - } - - // Accelerations at the CG - double longitudinal_acceleration = dynamic_state(0); - double lateral_acceleration = dynamic_state(1); - - double mass = this->car_parameters_->mass; - double g = 9.8065; // m/s^2 TODO: make this a constant in the car parameters (maybe?) - double cog_height = this->car_parameters_->cog_height; - double wheelbase = this->car_parameters_->wheelbase; - double front_weight_distribution = - this->car_parameters_->dist_cg_2_rear_axis / this->car_parameters_->wheelbase; - double rear_weight_distribution = 1.0 - front_weight_distribution; - double track_width = this->car_parameters_->track_width; - - // Static load per wheel - double Fz_total = mass * g; - double front_wheel_static_load = front_weight_distribution * Fz_total / 2.0; - double rear_wheel_static_load = rear_weight_distribution * Fz_total / 2.0; - - // Weight transfer for each axle - double longitudinal_transfer = (mass * longitudinal_acceleration * cog_height) / wheelbase; - double lateral_transfer = (mass * lateral_acceleration * cog_height) / track_width; - - Eigen::Vector4d loads; - - loads(0) = front_wheel_static_load - longitudinal_transfer / 2.0 - lateral_transfer / 2.0; - loads(1) = front_wheel_static_load - longitudinal_transfer / 2.0 + lateral_transfer / 2.0; - loads(2) = rear_wheel_static_load + longitudinal_transfer / 2.0 - lateral_transfer / 2.0; - loads(3) = rear_wheel_static_load + longitudinal_transfer / 2.0 + lateral_transfer / 2.0; - - return loads; -} \ No newline at end of file diff --git a/src/motion_lib/src/s2v_model/bicycle_model.cpp b/src/motion_lib/src/s2v_model/bicycle_model.cpp new file mode 100644 index 000000000..41fd0bbb3 --- /dev/null +++ b/src/motion_lib/src/s2v_model/bicycle_model.cpp @@ -0,0 +1,135 @@ +#include "motion_lib/s2v_model/bicycle_model.hpp" + +std::pair BicycleModel::wheels_velocities_to_cg(double rl_rpm, + [[maybe_unused]] double fl_rpm, + double rr_rpm, + [[maybe_unused]] double fr_rpm, + double steering_angle) { + std::pair velocities = {0, 0}; + double rl_velocity = + common_lib::maths::get_wheel_velocity_from_rpm(rl_rpm, this->car_parameters_.wheel_diameter); + double rr_velocity = + common_lib::maths::get_wheel_velocity_from_rpm(rr_rpm, this->car_parameters_.wheel_diameter); + if (steering_angle == 0) { // If no steering angle, moving straight + velocities.first = (rl_velocity + rr_velocity) / 2; + } else if (steering_angle > 0) { + double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); + velocities.second = + rl_velocity / (rear_axis_center_rotation_radius - (this->car_parameters_.track_width / 2)); + velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + + pow(this->car_parameters_.rear_axis_to_camera, 2)) * + fabs(velocities.second); + } else { + double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); + velocities.second = + rr_velocity / (rear_axis_center_rotation_radius + (this->car_parameters_.track_width / 2)); + velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + + pow(this->car_parameters_.rear_axis_to_camera, 2)) * + fabs(velocities.second); + } + return velocities; +} + +common_lib::structures::Position BicycleModel::rear_axis_position( + const common_lib::structures::Position& cg, double orientation, double dist_cg_2_rear_axis) { + common_lib::structures::Position rear_axis; + rear_axis.x = cg.x - dist_cg_2_rear_axis * cos(orientation); + rear_axis.y = cg.y - dist_cg_2_rear_axis * sin(orientation); + return rear_axis; +} + +Eigen::VectorXd BicycleModel::cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { + const double vx = cg_velocities(0); + const double vy = cg_velocities(1); + const double omega = cg_velocities(2); + const double lr = + this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels + const double lf = + this->car_parameters_.wheelbase - + this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels + + double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); + double rear_wheels_rpm = 60 * rear_wheel_velocity / (M_PI * this->car_parameters_.wheel_diameter); + if (vx < 0) { + rear_wheels_rpm = -rear_wheels_rpm; + } + + double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); + double front_wheels_rpm = + 60 * front_wheel_velocity / (M_PI * this->car_parameters_.wheel_diameter); + if (vx < 0) { + front_wheels_rpm = -front_wheels_rpm; + } + double v = sqrt(pow(vx, 2) + pow(vy, 2)); + double steering_angle = + (std::fabs(v) <= 0.1) ? 0 : atan(omega * (this->car_parameters_.wheelbase) / v); + double motor_rpm = 60 * this->car_parameters_.gear_ratio * rear_wheel_velocity / + (M_PI * this->car_parameters_.wheel_diameter); + if (vx < 0) { + motor_rpm = -motor_rpm; + } + + Eigen::VectorXd observations = Eigen::VectorXd::Zero(6); + observations << front_wheels_rpm, front_wheels_rpm, rear_wheels_rpm, rear_wheels_rpm, + steering_angle, motor_rpm; + return observations; +} + +Eigen::MatrixXd BicycleModel::jacobian_cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 3); + double vx = cg_velocities(0); + double vy = cg_velocities(1); + double omega = cg_velocities(2); + double lr = this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels + double lf = this->car_parameters_.wheelbase - + this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels + double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); + double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); + + const double epsilon = 1e-3; + if (std::fabs(rear_wheel_velocity) < epsilon || std::fabs(front_wheel_velocity) < epsilon) { + jacobian(0, 0) = epsilon; + return jacobian; + } + + double sign = (vx < 0) ? -1.0 : 1.0; + jacobian(0, 0) = + sign * 60 * vx / (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); + jacobian(0, 1) = sign * 60 * (vy + omega * lf) / + (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); + jacobian(0, 2) = sign * 60 * lf * (vy + omega * lf) / + (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); + jacobian(1, 0) = jacobian(0, 0); + jacobian(1, 1) = jacobian(0, 1); + jacobian(1, 2) = jacobian(0, 2); + + jacobian(2, 0) = + sign * 60 * vx / (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(2, 1) = sign * 60 * (vy - omega * lr) / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(2, 2) = sign * 60 * (-lr) * (vy - omega * lr) / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(3, 0) = jacobian(2, 0); + jacobian(3, 1) = jacobian(2, 1); + jacobian(3, 2) = jacobian(2, 2); + + jacobian(5, 0) = sign * this->car_parameters_.gear_ratio * 60 * vx / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(5, 1) = sign * this->car_parameters_.gear_ratio * 60 * (vy - omega * lr) / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(5, 2) = sign * this->car_parameters_.gear_ratio * 60 * (-lr) * (vy - omega * lr) / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + double L = this->car_parameters_.wheelbase; + double v = sqrt(pow(vx, 2) + pow(vy, 2)); + if (std::fabs(v) > epsilon) { + jacobian(4, 0) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vx / v); + jacobian(4, 1) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vy / v); + jacobian(4, 2) = (L * v) / (v * v + (omega * L) * (omega * L)); + } + + return jacobian; +} \ No newline at end of file diff --git a/src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp b/src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp new file mode 100644 index 000000000..78b1f1716 --- /dev/null +++ b/src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp @@ -0,0 +1,120 @@ +#include "motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp" + +std::pair NoRearWSSBicycleModel::wheels_velocities_to_cg( + double rl_rpm, [[maybe_unused]] double fl_rpm, double rr_rpm, [[maybe_unused]] double fr_rpm, + double steering_angle) { + std::pair velocities = {0, 0}; + double rl_velocity = + common_lib::maths::get_wheel_velocity_from_rpm(rl_rpm, this->car_parameters_.wheel_diameter); + double rr_velocity = + common_lib::maths::get_wheel_velocity_from_rpm(rr_rpm, this->car_parameters_.wheel_diameter); + if (steering_angle == 0) { // If no steering angle, moving straight + velocities.first = (rl_velocity + rr_velocity) / 2; + } else if (steering_angle > 0) { + double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); + velocities.second = + rl_velocity / (rear_axis_center_rotation_radius - (this->car_parameters_.track_width / 2)); + velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + + pow(this->car_parameters_.rear_axis_to_camera, 2)) * + fabs(velocities.second); + } else { + double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); + velocities.second = + rr_velocity / (rear_axis_center_rotation_radius + (this->car_parameters_.track_width / 2)); + velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + + pow(this->car_parameters_.rear_axis_to_camera, 2)) * + fabs(velocities.second); + } + return velocities; +} + +common_lib::structures::Position NoRearWSSBicycleModel::rear_axis_position( + const common_lib::structures::Position& cg, double orientation, double dist_cg_2_rear_axis) { + common_lib::structures::Position rear_axis; + rear_axis.x = cg.x - dist_cg_2_rear_axis * cos(orientation); + rear_axis.y = cg.y - dist_cg_2_rear_axis * sin(orientation); + return rear_axis; +} + +Eigen::VectorXd NoRearWSSBicycleModel::cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { + const double vx = cg_velocities(0); + const double vy = cg_velocities(1); + const double omega = cg_velocities(2); + const double lr = + this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels + const double lf = + this->car_parameters_.wheelbase - + this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels + + double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); + + double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); + double front_wheels_rpm = + 60 * front_wheel_velocity / (M_PI * this->car_parameters_.wheel_diameter); + if (vx < 0) { + front_wheels_rpm = -front_wheels_rpm; + } + double v = sqrt(pow(vx, 2) + pow(vy, 2)); + double steering_angle = + (std::fabs(v) <= 1e-2) ? 0 : atan(omega * (this->car_parameters_.wheelbase) / v); + double motor_rpm = 60 * this->car_parameters_.gear_ratio * rear_wheel_velocity / + (M_PI * this->car_parameters_.wheel_diameter); + if (vx < 0) { + motor_rpm = -motor_rpm; + } + + Eigen::VectorXd observations = Eigen::VectorXd::Zero(4); + observations << front_wheels_rpm, front_wheels_rpm, steering_angle, motor_rpm; + return observations; +} + +Eigen::MatrixXd NoRearWSSBicycleModel::jacobian_cg_velocity_to_wheels( + const Eigen::Vector3d& cg_velocities) { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(4, 3); + double vx = cg_velocities(0); + double vy = cg_velocities(1); + double omega = cg_velocities(2); + double lr = this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels + double lf = this->car_parameters_.wheelbase - + this->car_parameters_ + .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels + double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); + double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); + + const double epsilon = 1e-2; + if (std::fabs(rear_wheel_velocity) < epsilon || std::fabs(front_wheel_velocity) < epsilon) { + jacobian(3, 0) = epsilon; + return jacobian; + } + + double sign = (vx < 0) ? -1.0 : 1.0; + jacobian(0, 0) = std::max( + 0.0, sign * 60 * vx / (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity)); + jacobian(0, 1) = 0; + // std::max(0.0, sign * 60 * (vy + omega * lf) / + // (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity)); + jacobian(0, 2) = sign * 60 * lf * (vy + omega * lf) / + (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); + jacobian(1, 0) = jacobian(0, 0); + jacobian(1, 1) = jacobian(0, 1); + jacobian(1, 2) = jacobian(0, 2); + + jacobian(3, 0) = sign * this->car_parameters_.gear_ratio * 60 * vx / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(3, 1) = 0; // sign * this->car_parameters_.gear_ratio * 60 * (vy - omega * lr) / + //(M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + jacobian(3, 2) = sign * this->car_parameters_.gear_ratio * 60 * (-lr) * (vy - omega * lr) / + (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); + double L = this->car_parameters_.wheelbase; + double v = sqrt(pow(vx, 2) + pow(vy, 2)); + if (std::fabs(v) > epsilon) { + jacobian(2, 0) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vx / v); + jacobian(2, 1) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vy / v); + jacobian(2, 2) = (L * v) / (v * v + (omega * L) * (omega * L)); + } + + return jacobian; +} \ No newline at end of file diff --git a/src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp b/src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp new file mode 100644 index 000000000..f337c242b --- /dev/null +++ b/src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp @@ -0,0 +1,73 @@ +#include "motion_lib/s2v_model/no_wss_bicycle_model.hpp" + +std::pair NoWSSBicycleModel::wheels_velocities_to_cg(double rl_rpm, + [[maybe_unused]] double fl_rpm, + double rr_rpm, + [[maybe_unused]] double fr_rpm, + double steering_angle) { + std::pair velocities = {0, 0}; + double rl_velocity = + common_lib::maths::get_wheel_velocity_from_rpm(rl_rpm, this->car_parameters_.wheel_diameter); + double rr_velocity = + common_lib::maths::get_wheel_velocity_from_rpm(rr_rpm, this->car_parameters_.wheel_diameter); + if (steering_angle == 0) { // If no steering angle, moving straight + velocities.first = (rl_velocity + rr_velocity) / 2; + } else if (steering_angle > 0) { + double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); + velocities.second = + rl_velocity / (rear_axis_center_rotation_radius - (this->car_parameters_.track_width / 2)); + velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + + pow(this->car_parameters_.rear_axis_to_camera, 2)) * + fabs(velocities.second); + } else { + double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); + velocities.second = + rr_velocity / (rear_axis_center_rotation_radius + (this->car_parameters_.track_width / 2)); + velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + + pow(this->car_parameters_.rear_axis_to_camera, 2)) * + fabs(velocities.second); + } + return velocities; +} + +common_lib::structures::Position NoWSSBicycleModel::rear_axis_position( + const common_lib::structures::Position& cg, double orientation, double dist_cg_2_rear_axis) { + common_lib::structures::Position rear_axis; + rear_axis.x = cg.x - dist_cg_2_rear_axis * cos(orientation); + rear_axis.y = cg.y - dist_cg_2_rear_axis * sin(orientation); + return rear_axis; +} + +Eigen::VectorXd NoWSSBicycleModel::cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { + const double vx = cg_velocities(0); + const double omega = cg_velocities(2); + double steering_angle = + (std::fabs(vx) <= 1e-2) ? 0 : atan(omega * (this->car_parameters_.wheelbase) / vx); + double motor_rpm = + 60 * this->car_parameters_.gear_ratio * vx / (M_PI * this->car_parameters_.wheel_diameter); + + Eigen::VectorXd observations = Eigen::VectorXd::Zero(2); + observations << steering_angle, motor_rpm; + return observations; +} + +Eigen::MatrixXd NoWSSBicycleModel::jacobian_cg_velocity_to_wheels( + const Eigen::Vector3d& cg_velocities) { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(2, 3); + double vx = cg_velocities(0); + double omega = cg_velocities(2); + double common_term = (std::fabs(vx) <= 1e-2) + ? 0 + : 1 / (1 + pow((this->car_parameters_.wheelbase * omega) / vx, 2)); + + jacobian(0, 0) = + common_term * + (std::fabs(vx) <= 1e-2 ? 0 : -this->car_parameters_.wheelbase * omega / (vx * vx)); + jacobian(0, 1) = 0; + jacobian(0, 2) = common_term * (std::fabs(vx) <= 1e-2 ? 0 : this->car_parameters_.wheelbase / vx); + jacobian(1, 0) = + 60 * this->car_parameters_.gear_ratio / (M_PI * this->car_parameters_.wheel_diameter); + jacobian(1, 1) = 0; + jacobian(1, 2) = 0; + return jacobian; +} \ No newline at end of file diff --git a/src/motion_lib/src/steering_model/parallel_front_steering.cpp b/src/motion_lib/src/steering_model/parallel_front_steering.cpp deleted file mode 100644 index 3fa4d1dca..000000000 --- a/src/motion_lib/src/steering_model/parallel_front_steering.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "motion_lib/steering_model/parallel_front_steering.hpp" - -Eigen::Vector4d ParallelFrontSteering::calculate_steering_angles( - double steering_wheel_angle) const { - return Eigen::Vector4d(steering_wheel_angle, steering_wheel_angle, 0, 0); -} \ No newline at end of file diff --git a/src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp b/src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp deleted file mode 100644 index 669d949ce..000000000 --- a/src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "motion_lib/steering_motor_model/pid_steering_motor.hpp" - -double PIDSteeringMotor::compute_steering_rate(double current_steering, double steering_goal) { - if (this->last_update == rclcpp::Time(0, 0)) { - this->last_update = rclcpp::Clock().now(); - return 0; - } - double dt = (rclcpp::Clock().now() - this->last_update).seconds(); - double error = steering_goal - current_steering; - - double proportional = error; - - integral_ += error * dt; - - double derivative = (error - previous_error_) / dt; - previous_error_ = error; - - // PID control output - double output = this->car_parameters_->steering_motor_parameters.kp * proportional + - this->car_parameters_->steering_motor_parameters.ki * integral_ + - this->car_parameters_->steering_motor_parameters.kd * derivative; - - return output; -} \ No newline at end of file diff --git a/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp b/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp deleted file mode 100644 index 022c8db33..000000000 --- a/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "motion_lib/tire_model/pacejka_combined_slip.hpp" - -std::pair PacejkaCombinedSlip::tire_forces(double slip_angle, double slip_ratio, - double vertical_load) const { - // Longitudinal pure slip force (Fx0) - double Bx = this->car_parameters_->tire_parameters.tire_B_longitudinal; - double Cx = this->car_parameters_->tire_parameters.tire_C_longitudinal; - double Dx = this->car_parameters_->tire_parameters.tire_D_longitudinal * vertical_load; - double Ex = this->car_parameters_->tire_parameters.tire_E_longitudinal; - - double Fx0 = - Dx * std::sin(Cx * std::atan(Bx * slip_ratio - Ex * (Bx * slip_ratio - std::atan(Bx * slip_ratio)))); - - // Lateral pure slip force (Fy0) - double By = this->car_parameters_->tire_parameters.tire_B_lateral; - double Cy = this->car_parameters_->tire_parameters.tire_C_lateral; - double Dy = this->car_parameters_->tire_parameters.tire_D_lateral * vertical_load; - double Ey = this->car_parameters_->tire_parameters.tire_E_lateral; - - double Fy0 = - Dy * std::sin(Cy * std::atan(By * slip_angle - Ey * (By * slip_angle - std::atan(By * slip_angle)))); - - // Combined slip reduction factors (friction ellipse type) - double Gx = std::cos(std::atan(By * slip_angle)); - double Gy = std::cos(std::atan(Bx * slip_ratio)); - - // Final forces with combined slip - double Fx = Fx0 * Gx; - double Fy = Fy0 * Gy; - - return std::make_pair(Fx, Fy); -} \ No newline at end of file diff --git a/src/motion_lib/src/v2p_models/base_v2p_motion_model.cpp b/src/motion_lib/src/v2p_models/base_v2p_motion_model.cpp index 7e693cbca..acd8f3744 100644 --- a/src/motion_lib/src/v2p_models/base_v2p_motion_model.cpp +++ b/src/motion_lib/src/v2p_models/base_v2p_motion_model.cpp @@ -2,22 +2,11 @@ #include "common_lib/maths/transformations.hpp" -V2PMotionModel::V2PMotionModel(const Eigen::Vector3d base_process_noise) - : _base_process_noise_(base_process_noise) {} - -Eigen::Vector3d V2PMotionModel::get_next_pose(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, - const double delta_t) { +Eigen::Vector3d V2PMotionModel::get_next_pose( + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { // TODO: change to XD and create new acceleration model Eigen::Vector3d next_pose = - previous_pose + this->get_pose_difference(previous_pose, velocities, delta_t); + previous_pose + this->get_pose_difference(previous_pose, motion_data, delta_t); next_pose(2) = common_lib::maths::normalize_angle(next_pose(2)); return next_pose; } - -V2PMotionModel::V2PMotionModel() : _base_process_noise_(Eigen::Vector3d::Zero()) {} - -Eigen::Matrix3d V2PMotionModel::get_base_process_noise_matrix() { - return _base_process_noise_.asDiagonal(); -} - -Eigen::Vector3d V2PMotionModel::get_base_process_noise() { return _base_process_noise_; } \ No newline at end of file diff --git a/src/motion_lib/src/v2p_models/constant_acceleration_turnrate_model.cpp b/src/motion_lib/src/v2p_models/constant_acceleration_turnrate_model.cpp new file mode 100644 index 000000000..e48987f79 --- /dev/null +++ b/src/motion_lib/src/v2p_models/constant_acceleration_turnrate_model.cpp @@ -0,0 +1,105 @@ + +#include "motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp" + +#include "common_lib/maths/transformations.hpp" + +Eigen::Vector3d ConstantAccelerationTurnrateModel::get_pose_difference( + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { + Eigen::Vector3d pose_difference; + if (::abs(motion_data(2)) < 0.0001) { // Avoid division by zero when car is going straight + pose_difference(0) = (motion_data(0) * delta_t + motion_data(3) * ::pow(delta_t, 2) / 2) * + ::cos(previous_pose(2)); + pose_difference(1) = (motion_data(0) * delta_t + motion_data(3) * ::pow(delta_t, 2) / 2) * + ::sin(previous_pose(2)); + } else { + pose_difference(0) = + ((motion_data(0) * motion_data(2) + motion_data(3) * motion_data(2) * delta_t) * + ::sin(previous_pose(2) + motion_data(2) * delta_t) + + motion_data(3) * ::cos(previous_pose(2) + motion_data(2) * delta_t) - + motion_data(0) * motion_data(2) * ::sin(previous_pose(2)) - + motion_data(3) * ::cos(previous_pose(2))) / + ::pow(motion_data(2), 2); + pose_difference(1) = + ((-motion_data(0) * motion_data(2) - motion_data(3) * motion_data(2) * delta_t) * + ::cos(previous_pose(2) + motion_data(2) * delta_t) + + motion_data(3) * ::sin(previous_pose(2) + motion_data(2) * delta_t) + + motion_data(0) * motion_data(2) * ::cos(previous_pose(2)) - + motion_data(3) * ::sin(previous_pose(2))) / + ::pow(motion_data(2), 2); + } + pose_difference(2) = common_lib::maths::normalize_angle(motion_data(2) * delta_t); + return pose_difference; +} + +Eigen::Matrix3d ConstantAccelerationTurnrateModel::get_jacobian_pose( + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { + Eigen::Matrix3d jacobian = Eigen::Matrix3d::Identity(); + if (::abs(motion_data(2)) < 0.001) { // Avoid division by zero when car is going straight + jacobian(0, 2) = -(motion_data(0) * delta_t + motion_data(3) * ::pow(delta_t, 2) / 2) * + ::sin(previous_pose(2)); + jacobian(1, 2) = (motion_data(0) * delta_t + motion_data(3) * ::pow(delta_t, 2) / 2) * + ::cos(previous_pose(2)); + } else { + jacobian(0, 2) = + (-motion_data(3) * ::sin(previous_pose(2) + motion_data(2) * delta_t) + + (motion_data(0) * motion_data(2) + motion_data(3) * motion_data(2) * delta_t) * + ::cos(previous_pose(2) + motion_data(2) * delta_t) + + motion_data(3) * ::sin(previous_pose(2)) - + motion_data(0) * motion_data(2) * ::cos(previous_pose(2))) / + (motion_data(2) * motion_data(2)); + jacobian(1, 2) = ((motion_data(0) + motion_data(3) * delta_t) * motion_data(2) * + ::sin(previous_pose(2) + motion_data(2) * delta_t) + + motion_data(3) * ::cos(previous_pose(2) + motion_data(2) * delta_t) - + motion_data(0) * motion_data(2) * ::sin(previous_pose(2)) - + motion_data(3) * ::cos(previous_pose(2))) / + (motion_data(2) * motion_data(2)); + } + return jacobian; +} + +Eigen::MatrixXd ConstantAccelerationTurnrateModel::get_jacobian_motion_data( + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(3, 4); + if (::abs(motion_data(2)) < 0.001) { + jacobian(0, 0) = ::cos(previous_pose(2)) * delta_t; + jacobian(0, 3) = ::cos(previous_pose(2)) * ::pow(delta_t, 2) / 2; + jacobian(1, 0) = ::sin(previous_pose(2)) * delta_t; + jacobian(1, 3) = ::sin(previous_pose(2)) * ::pow(delta_t, 2) / 2; + } else { + jacobian(0, 0) = + (::sin(motion_data(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))) / + motion_data(2); + jacobian(0, 2) = -(((-delta_t * motion_data(0) - ::pow(delta_t, 2) * motion_data(3)) * + ::pow(motion_data(2), 2) + + 2 * motion_data(3)) * + ::cos(motion_data(2) * delta_t + previous_pose(2)) + + (motion_data(0) + 2 * motion_data(3) * delta_t) * motion_data(2) * + ::sin(delta_t * motion_data(2) + previous_pose(2)) - + ::sin(previous_pose(2)) * motion_data(0) * motion_data(2) - + 2 * motion_data(3) * ::cos(previous_pose(2))) / + ::pow(motion_data(2), 3); + jacobian(0, 3) = delta_t * motion_data(2) * ::sin(motion_data(2) * delta_t + previous_pose(2)) + + ::cos(motion_data(2) * delta_t + previous_pose(2) - ::cos(previous_pose(2))) / + ::pow(motion_data(2), 2); + jacobian(1, 0) = + -(::cos(motion_data(2) * delta_t + previous_pose(2)) - ::cos(previous_pose(2))) / + motion_data(2); + jacobian(1, 2) = (((delta_t * motion_data(0) + ::pow(delta_t, 2) * motion_data(3)) * + ::pow(motion_data(2), 2) - + 2 * motion_data(3)) * + ::sin(motion_data(2) * delta_t + previous_pose(2)) + + (motion_data(0) + 2 * motion_data(3) * delta_t) * motion_data(2) * + ::cos(delta_t * motion_data(2) + previous_pose(2)) - + ::cos(previous_pose(2)) * motion_data(0) * motion_data(2) + + 2 * motion_data(3) * ::sin(previous_pose(2))) / + ::pow(motion_data(2), 3); + jacobian(1, 3) = delta_t * motion_data(2) * ::cos(motion_data(2) * delta_t + previous_pose(2)) + + ::sin(motion_data(2) * delta_t + previous_pose(2) - ::sin(previous_pose(2))) / + ::pow(motion_data(2), 2); + } + jacobian(2, 2) = delta_t; + return jacobian; +} \ No newline at end of file diff --git a/src/motion_lib/src/v2p_models/constant_velocity_model.cpp b/src/motion_lib/src/v2p_models/constant_velocity_model.cpp index edb0dacdd..5b2cfbaba 100644 --- a/src/motion_lib/src/v2p_models/constant_velocity_model.cpp +++ b/src/motion_lib/src/v2p_models/constant_velocity_model.cpp @@ -3,39 +3,38 @@ #include "common_lib/maths/transformations.hpp" -ConstantVelocityModel::ConstantVelocityModel(const Eigen::Vector3d base_process_noise) - : V2PMotionModel(base_process_noise) {} - -ConstantVelocityModel::ConstantVelocityModel() : V2PMotionModel() {} Eigen::Vector3d ConstantVelocityModel::get_pose_difference(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) { Eigen::Vector3d pose_difference; pose_difference(0) = - (velocities(0) * ::cos(previous_pose(2)) - velocities(1) * ::sin(previous_pose(2))) * delta_t; + (motion_data(0) * ::cos(previous_pose(2)) - motion_data(1) * ::sin(previous_pose(2))) * + delta_t; pose_difference(1) = - (velocities(0) * ::sin(previous_pose(2)) + velocities(1) * ::cos(previous_pose(2))) * delta_t; - pose_difference(2) = common_lib::maths::normalize_angle(velocities(2) * delta_t); + (motion_data(0) * ::sin(previous_pose(2)) + motion_data(1) * ::cos(previous_pose(2))) * + delta_t; + pose_difference(2) = common_lib::maths::normalize_angle(motion_data(2) * delta_t); return pose_difference; } Eigen::Matrix3d ConstantVelocityModel::get_jacobian_pose(const Eigen::Vector3d &previous_pose, - const Eigen::Vector3d &velocities, + const Eigen::VectorXd &motion_data, const double delta_t) { Eigen::Matrix3d jacobian = Eigen::Matrix3d::Identity(); jacobian(0, 2) = - -(velocities(0) * ::sin(previous_pose(2)) + velocities(1) * ::cos(previous_pose(2))) * + -(motion_data(0) * ::sin(previous_pose(2)) + motion_data(1) * ::cos(previous_pose(2))) * delta_t; jacobian(1, 2) = - (velocities(0) * ::cos(previous_pose(2)) - velocities(1) * ::sin(previous_pose(2))) * delta_t; + (motion_data(0) * ::cos(previous_pose(2)) - motion_data(1) * ::sin(previous_pose(2))) * + delta_t; return jacobian; } -Eigen::Matrix3d ConstantVelocityModel::get_jacobian_velocities( - const Eigen::Vector3d &previous_pose, [[maybe_unused]] const Eigen::Vector3d &velocities, +Eigen::MatrixXd ConstantVelocityModel::get_jacobian_motion_data( + const Eigen::Vector3d &previous_pose, [[maybe_unused]] const Eigen::VectorXd &motion_data, const double delta_t) { - Eigen::Matrix3d jacobian = Eigen::Matrix3d::Zero(); + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(3, 4); jacobian(0, 0) = ::cos(previous_pose(2)) * delta_t; jacobian(0, 1) = -::sin(previous_pose(2)) * delta_t; jacobian(1, 0) = ::sin(previous_pose(2)) * delta_t; diff --git a/src/motion_lib/src/v2p_models/constant_velocity_turnrate_model.cpp b/src/motion_lib/src/v2p_models/constant_velocity_turnrate_model.cpp index f789da018..0a502db9d 100644 --- a/src/motion_lib/src/v2p_models/constant_velocity_turnrate_model.cpp +++ b/src/motion_lib/src/v2p_models/constant_velocity_turnrate_model.cpp @@ -3,77 +3,66 @@ #include "common_lib/maths/transformations.hpp" -ConstantVelocityTurnrateModel::ConstantVelocityTurnrateModel( - const Eigen::Vector3d base_process_noise) - : V2PMotionModel(base_process_noise) {} - -ConstantVelocityTurnrateModel::ConstantVelocityTurnrateModel() : V2PMotionModel() {} - Eigen::Vector3d ConstantVelocityTurnrateModel::get_pose_difference( - const Eigen::Vector3d &previous_pose, const Eigen::Vector3d &velocities, const double delta_t) { + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { Eigen::Vector3d pose_difference; - if (::abs(velocities(2)) < 0.0001) { // Avoid division by zero when car is going straight - pose_difference(0) = - (velocities(0) * ::cos(previous_pose(2)) - velocities(1) * ::sin(previous_pose(2))) * - delta_t; - pose_difference(1) = - (velocities(0) * ::sin(previous_pose(2)) + velocities(1) * ::cos(previous_pose(2))) * - delta_t; + if (::abs(motion_data(2)) < 0.0001) { // Avoid division by zero when car is going straight + pose_difference(0) = motion_data(0) * ::cos(previous_pose(2)) * delta_t; + pose_difference(1) = motion_data(0) * ::sin(previous_pose(2)) * delta_t; } else { pose_difference(0) = - velocities(0) / velocities(2) * - (::sin(velocities(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))); + motion_data(0) / motion_data(2) * + (::sin(motion_data(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))); pose_difference(1) = - velocities(0) / velocities(2) * - (-::cos(velocities(2) * delta_t + previous_pose(2)) + ::cos(previous_pose(2))); + motion_data(0) / motion_data(2) * + (-::cos(motion_data(2) * delta_t + previous_pose(2)) + ::cos(previous_pose(2))); } - pose_difference(2) = common_lib::maths::normalize_angle(velocities(2) * delta_t); + pose_difference(2) = common_lib::maths::normalize_angle(motion_data(2) * delta_t); return pose_difference; } Eigen::Matrix3d ConstantVelocityTurnrateModel::get_jacobian_pose( - const Eigen::Vector3d &previous_pose, const Eigen::Vector3d &velocities, const double delta_t) { + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { Eigen::Matrix3d jacobian = Eigen::Matrix3d::Identity(); - if (::abs(velocities(2)) < 0.001) { // Avoid division by zero when car is going straight - jacobian(0, 2) = - -(velocities(0) * ::sin(previous_pose(2)) + velocities(1) * ::cos(previous_pose(2))) * - delta_t; - jacobian(1, 2) = - (velocities(0) * ::cos(previous_pose(2)) - velocities(1) * ::sin(previous_pose(2))) * - delta_t; + if (::abs(motion_data(2)) < 0.001) { // Avoid division by zero when car is going straight + jacobian(0, 2) = -motion_data(0) * ::sin(previous_pose(2)) * delta_t; + jacobian(1, 2) = motion_data(0) * ::cos(previous_pose(2)) * delta_t; } else { - jacobian(0, 2) = velocities(0) / velocities(2) * - (::cos(velocities(2) * delta_t + previous_pose(2)) - ::cos(previous_pose(2))); - jacobian(1, 2) = velocities(0) / velocities(2) * - (::sin(velocities(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))); + jacobian(0, 2) = motion_data(0) / motion_data(2) * + (::cos(motion_data(2) * delta_t + previous_pose(2)) - ::cos(previous_pose(2))); + jacobian(1, 2) = motion_data(0) / motion_data(2) * + (::sin(motion_data(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))); } return jacobian; } -Eigen::Matrix3d ConstantVelocityTurnrateModel::get_jacobian_velocities( - const Eigen::Vector3d &previous_pose, const Eigen::Vector3d &velocities, const double delta_t) { - Eigen::Matrix3d jacobian = Eigen::Matrix3d::Zero(); - if (::abs(velocities(2)) < 0.001) { +Eigen::MatrixXd ConstantVelocityTurnrateModel::get_jacobian_motion_data( + const Eigen::Vector3d &previous_pose, const Eigen::VectorXd &motion_data, + const double delta_t) { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(3, 4); + if (::abs(motion_data(2)) < 0.001) { jacobian(0, 0) = ::cos(previous_pose(2)) * delta_t; - jacobian(0, 1) = -::sin(previous_pose(2)) * delta_t; jacobian(1, 0) = ::sin(previous_pose(2)) * delta_t; - jacobian(1, 1) = ::cos(previous_pose(2)) * delta_t; } else { - jacobian(0, 0) = 1 / velocities(2) * - (::sin(velocities(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))); + jacobian(0, 0) = 1 / motion_data(2) * + (::sin(motion_data(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))); jacobian(0, 2) = - -(velocities(0) * - (::sin(velocities(2) * delta_t + previous_pose(2)) - - velocities(2) * delta_t * ::cos(velocities(2) * delta_t + previous_pose(2)) - + -(motion_data(0) * + (::sin(motion_data(2) * delta_t + previous_pose(2)) - + motion_data(2) * delta_t * ::cos(motion_data(2) * delta_t + previous_pose(2)) - ::sin(previous_pose(2))) / - (velocities(2) * velocities(2))); - jacobian(1, 0) = 1 / velocities(2) * - (-::cos(velocities(2) * delta_t + previous_pose(2)) + ::cos(previous_pose(2))); - jacobian(1, 2) = (velocities(0) * - (::cos(velocities(2) * delta_t + previous_pose(2)) + - velocities(2) * delta_t * ::sin(velocities(2) * delta_t + previous_pose(2)) - - ::cos(previous_pose(2))) / - (velocities(2) * velocities(2))); + (motion_data(2) * motion_data(2))); + jacobian(1, 0) = + 1 / motion_data(2) * + (-::cos(motion_data(2) * delta_t + previous_pose(2)) + ::cos(previous_pose(2))); + jacobian(1, 2) = + (motion_data(0) * + (::cos(motion_data(2) * delta_t + previous_pose(2)) + + motion_data(2) * delta_t * ::sin(motion_data(2) * delta_t + previous_pose(2)) - + ::cos(previous_pose(2))) / + (motion_data(2) * motion_data(2))); } jacobian(2, 2) = delta_t; return jacobian; diff --git a/src/motion_lib/src/v2p_models/odometry_model.cpp b/src/motion_lib/src/v2p_models/odometry_model.cpp new file mode 100644 index 000000000..5a34dec23 --- /dev/null +++ b/src/motion_lib/src/v2p_models/odometry_model.cpp @@ -0,0 +1,31 @@ + +#include "motion_lib/v2p_models/odometry_model.hpp" + +#include "common_lib/maths/transformations.hpp" + +Eigen::Vector3d OdometryModel::get_pose_difference(const Eigen::Vector3d &previous_pose, + const Eigen::VectorXd &motion_data, + [[maybe_unused]] const double delta_t) { + Eigen::Vector3d pose_difference; + pose_difference(0) = motion_data(0) - previous_pose(0); + pose_difference(1) = motion_data(1) - previous_pose(1); + pose_difference(2) = common_lib::maths::normalize_angle(motion_data(2) - previous_pose(2)); + return pose_difference; +} + +Eigen::Matrix3d OdometryModel::get_jacobian_pose( + [[maybe_unused]] const Eigen::Vector3d &previous_pose, + [[maybe_unused]] const Eigen::VectorXd &motion_data, [[maybe_unused]] const double delta_t) { + Eigen::Matrix3d jacobian = Eigen::Matrix3d::Identity(); + return jacobian; +} + +Eigen::MatrixXd OdometryModel::get_jacobian_motion_data( + [[maybe_unused]] const Eigen::Vector3d &previous_pose, + [[maybe_unused]] const Eigen::VectorXd &motion_data, [[maybe_unused]] const double delta_t) { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(3, 4); + jacobian(0, 0) = 1.0; + jacobian(1, 1) = 1.0; + jacobian(2, 2) = 1.0; + return jacobian; +} \ No newline at end of file diff --git a/src/motion_lib/src/vel_process_model/particle_model.cpp b/src/motion_lib/src/vel_process_model/particle_model.cpp index 3c26c68aa..8b8eb27fa 100644 --- a/src/motion_lib/src/vel_process_model/particle_model.cpp +++ b/src/motion_lib/src/vel_process_model/particle_model.cpp @@ -24,5 +24,14 @@ Eigen::Matrix3d CAParticleModel::get_jacobian_velocities( jacobian_matrix(1, 0) = -previous_velocities(2) * time_interval; jacobian_matrix(1, 2) = -previous_velocities(0) * time_interval; + return jacobian_matrix; +} + +Eigen::Matrix3d CAParticleModel::get_jacobian_sensor_data( + [[maybe_unused]] const Eigen::Vector3d& previous_velocities, + [[maybe_unused]] const Eigen::Vector3d& accelerations, const double time_interval) { + Eigen::Matrix3d jacobian_matrix = Eigen::Matrix3d::Zero(); + jacobian_matrix(0, 0) = time_interval; + jacobian_matrix(1, 1) = time_interval; return jacobian_matrix; } \ No newline at end of file diff --git a/src/perception_sensor_lib/test/observation_model/ve_observation_model/no_slip_bicycle_model_test.cpp b/src/motion_lib/test/s2v_models/bicycle_model_test.cpp similarity index 70% rename from src/perception_sensor_lib/test/observation_model/ve_observation_model/no_slip_bicycle_model_test.cpp rename to src/motion_lib/test/s2v_models/bicycle_model_test.cpp index 3792f08db..23519535b 100644 --- a/src/perception_sensor_lib/test/observation_model/ve_observation_model/no_slip_bicycle_model_test.cpp +++ b/src/motion_lib/test/s2v_models/bicycle_model_test.cpp @@ -1,4 +1,4 @@ -#include "perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp" +#include "motion_lib/s2v_model/bicycle_model.hpp" #include @@ -6,19 +6,65 @@ /* ----------------------- ODOMETRY MODEL -------------------------*/ +/** + * @brief Tests the conversion from wheel revolutions + * to velocities using the bycicle model + * + */ +TEST(ODOMETRY_SUBSCRIBER, CONVERSION_TEST) { + // Straight Line + BicycleModel bicycle_model = + BicycleModel(common_lib::car_parameters::CarParameters(0.516, 1.6, 0.79, 1.2, 0.791, 4.0)); + double rl_speed = 60; + double rr_speed = 60; + double fl_speed = 60; + double fr_speed = 60; + double steering_angle = 0; + std::pair velocity_data = + bicycle_model.wheels_velocities_to_cg(rl_speed, rr_speed, fl_speed, fr_speed, steering_angle); + EXPECT_NEAR(velocity_data.first, 1.6210, 0.0001); + EXPECT_DOUBLE_EQ(velocity_data.second, 0); + + // Curving left + rl_speed = 60; + rr_speed = 60; + fl_speed = 60; + fr_speed = 60; + steering_angle = M_PI / 8; + velocity_data = + bicycle_model.wheels_velocities_to_cg(rl_speed, rr_speed, fl_speed, fr_speed, steering_angle); + EXPECT_GE(velocity_data.first, 1.6210); + EXPECT_LE(velocity_data.first, 1.6210 * 2); + EXPECT_LE(velocity_data.second, M_PI); + EXPECT_GE(velocity_data.second, M_PI / 8); + + // Curving right + rl_speed = 60; + rr_speed = 60; + fl_speed = 60; + fr_speed = 60; + steering_angle = -M_PI / 8; + velocity_data = + bicycle_model.wheels_velocities_to_cg(rl_speed, rr_speed, fl_speed, fr_speed, steering_angle); + EXPECT_GE(velocity_data.first, 1.6210); + EXPECT_LE(velocity_data.first, 1.6210 * 2); + EXPECT_GE(velocity_data.second, -M_PI); + EXPECT_LE(velocity_data.second, -M_PI / 8); +} + /** * @brief Test a regular case of the conversion from wheel velocities to cg velocities * */ -TEST(NoSlipBicycleModelTest, TestCgVelocityToWheels) { +TEST(BicycleModelTest, TestCgVelocityToWheels) { common_lib::car_parameters::CarParameters car_parameters_; car_parameters_.dist_cg_2_rear_axis = 1.0; car_parameters_.wheelbase = 2.5; car_parameters_.wheel_diameter = 0.6; car_parameters_.gear_ratio = 4.0; - NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + BicycleModel bicycle_model = BicycleModel(car_parameters_); Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); // Example velocities - Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities); ASSERT_EQ(observations.size(), 6); @@ -35,15 +81,15 @@ TEST(NoSlipBicycleModelTest, TestCgVelocityToWheels) { * when the velocity in x is negative (the car is moving backwards) * */ -TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsNegativeVx) { +TEST(BicycleModelTest, TestCgVelocityToWheelsNegativeVx) { common_lib::car_parameters::CarParameters car_parameters_; car_parameters_.dist_cg_2_rear_axis = 1.0; car_parameters_.wheelbase = 2.5; car_parameters_.wheel_diameter = 0.6; car_parameters_.gear_ratio = 4.0; - NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + BicycleModel bicycle_model = BicycleModel(car_parameters_); Eigen::Vector3d cg_velocities(-10.0, 2.0, 0.1); // Example velocities - Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities); // Check the size of the observations vector ASSERT_EQ(observations.size(), 6); @@ -61,15 +107,15 @@ TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsNegativeVx) { * when the velocity in x is null and the other velocities are negative * */ -TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVx) { +TEST(BicycleModelTest, TestCgVelocityToWheelsZeroVx) { common_lib::car_parameters::CarParameters car_parameters_; car_parameters_.dist_cg_2_rear_axis = 1.0; car_parameters_.wheelbase = 2.5; car_parameters_.wheel_diameter = 0.6; car_parameters_.gear_ratio = 4.0; - NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + BicycleModel bicycle_model = BicycleModel(car_parameters_); Eigen::Vector3d cg_velocities(0.0, -2.0, -0.1); // Example velocities - Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities); // Check the size of the observations vector ASSERT_EQ(observations.size(), 6); @@ -87,15 +133,15 @@ TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVx) { * when the velocities are null * */ -TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVy) { +TEST(BicycleModelTest, TestCgVelocityToWheelsZeroVy) { common_lib::car_parameters::CarParameters car_parameters_; car_parameters_.dist_cg_2_rear_axis = 1.0; car_parameters_.wheelbase = 2.5; car_parameters_.wheel_diameter = 0.6; car_parameters_.gear_ratio = 4.0; - NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + BicycleModel bicycle_model = BicycleModel(car_parameters_); Eigen::Vector3d cg_velocities(0.0, 0.0, 0.0); // Example velocities - Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(cg_velocities); // Check the size of the observations vector ASSERT_EQ(observations.size(), 6); @@ -113,7 +159,7 @@ TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVy) { * with regular values of the velocities * */ -TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheels) { +TEST(BicycleModelTest, TestJacobianCgVelocityToWheels) { // Initialize car parameters common_lib::car_parameters::CarParameters car_parameters; car_parameters.dist_cg_2_rear_axis = 1.0; @@ -121,14 +167,14 @@ TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheels) { car_parameters.wheel_diameter = 0.6; car_parameters.gear_ratio = 4.0; - // Initialize NoSlipBicycleModel with car parameters - NoSlipBicycleModel bicycle_model(car_parameters); + // Initialize BicycleModel with car parameters + BicycleModel bicycle_model(car_parameters); // Example velocities Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); // Calculate the Jacobian - Eigen::MatrixXd jacobian = bicycle_model.expected_observations_jacobian(cg_velocities); + Eigen::MatrixXd jacobian = bicycle_model.jacobian_cg_velocity_to_wheels(cg_velocities); // Check the size of the Jacobian matrix ASSERT_EQ(jacobian.rows(), 6); @@ -160,7 +206,7 @@ TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheels) { * and the angular velocity is negative * */ -TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheelsZeroVy) { +TEST(BicycleModelTest, TestJacobianCgVelocityToWheelsZeroVy) { // Initialize car parameters common_lib::car_parameters::CarParameters car_parameters; car_parameters.dist_cg_2_rear_axis = 1.0; @@ -168,14 +214,14 @@ TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheelsZeroVy) { car_parameters.wheel_diameter = 0.6; car_parameters.gear_ratio = 4.0; - // Initialize NoSlipBicycleModel with car parameters - NoSlipBicycleModel bicycle_model(car_parameters); + // Initialize BicycleModel with car parameters + BicycleModel bicycle_model(car_parameters); // Example velocities Eigen::Vector3d cg_velocities(-10.0, 0, -0.1); // Calculate the Jacobian - Eigen::MatrixXd jacobian = bicycle_model.expected_observations_jacobian(cg_velocities); + Eigen::MatrixXd jacobian = bicycle_model.jacobian_cg_velocity_to_wheels(cg_velocities); // Check the size of the Jacobian matrix ASSERT_EQ(jacobian.rows(), 6); diff --git a/src/motion_lib/test/v2p_models/constant_acceleration_turnrate_model_test.cpp b/src/motion_lib/test/v2p_models/constant_acceleration_turnrate_model_test.cpp new file mode 100644 index 000000000..d078a52fd --- /dev/null +++ b/src/motion_lib/test/v2p_models/constant_acceleration_turnrate_model_test.cpp @@ -0,0 +1,400 @@ +#include "motion_lib/v2p_models/constant_acceleration_turnrate_model.hpp" + +#include + +#include + +// TODO: improve tests: have tests be based off of a continuous example, with data regarding a +// vehicle performing a complex movement or something similar. + +/** + * @brief Test the constant acceleration and turnrate model with a straight line movement in the x + * axis + * - positive vx + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_1) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, 0); + Eigen::Vector4d motion_data(1, 0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 1, 0.000001); + EXPECT_NEAR(next_pose(1), 0, 0.000001); + EXPECT_NEAR(next_pose(2), 0, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(0, 2), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_FLOAT_EQ(pose_jacobian(1, 2), 1); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model with a straight line movement in the x + * axis + * - positive vx + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, STRAIGHT_LINE_MOVEMENT_TEST_2) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, M_PI / 4); + Eigen::Vector4d motion_data(2, 0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 1.41, 0.01); + EXPECT_NEAR(next_pose(1), 1.41, 0.01); + EXPECT_NEAR(next_pose(2), M_PI / 4, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), -1.41, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 1.41, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model with a backwards movement in the x axis + * - negative vx + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_1) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, 0.0); + Eigen::Vector4d motion_data(-1, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), -1.0, 0.000001); + EXPECT_NEAR(next_pose(1), 0, 0.000001); + EXPECT_NEAR(abs(next_pose(2)), 0.0, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model with a backwards movement in the x axis + * - positive vx (paradox) + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_2) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, M_PI); + Eigen::Vector4d motion_data(1, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), -1.00, 0.000001); + EXPECT_NEAR(next_pose(1), 0, 0.000001); + EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), -1, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model with a backwards movement in the x axis + * - negative vy (paradox) + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, BACKWARDS_MOVEMENT_TEST_3) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, -M_PI); + Eigen::Vector4d motion_data(-1, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 1.00, 0.000001); + EXPECT_NEAR(next_pose(1), 0, 0.000001); + EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 1, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model for angle cap on the orientation update + * - 2PI + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_1) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, 2 * M_PI); + Eigen::Vector4d motion_data(0.0, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 0.0, 0.001); + EXPECT_NEAR(next_pose(1), 0.0, 0.000001); + EXPECT_NEAR(next_pose(2), 0.0, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model for angle cap on the orientation update + * - 3PI + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_2) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, 3 * M_PI); + Eigen::Vector4d motion_data(0.0, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 0.0, 0.001); + EXPECT_NEAR(next_pose(1), 0.0, 0.000001); + EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model for angle cap on the orientation update + * - -3PI + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_3) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, -3 * M_PI); + Eigen::Vector4d motion_data(0.0, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 0.0, 0.001); + EXPECT_NEAR(next_pose(1), 0.0, 0.000001); + EXPECT_NEAR(abs(next_pose(2)), M_PI, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model for angle cap on the orientation update + * - 3PI/2 + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_4) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, 1.5 * M_PI); + Eigen::Vector4d motion_data(0.0, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 0.0, 0.001); + EXPECT_NEAR(next_pose(1), 0.0, 0.000001); + EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model for angle cap on the orientation update + * - -5PI/2 + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, ORIENTATION_ANGLE_CAP_TEST_5) { + // Arrange + Eigen::Vector3d previous_pose(0, 0, -2.5 * M_PI); + Eigen::Vector4d motion_data(0.0, 0.0, 0, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 0.0, 0.001); + EXPECT_NEAR(next_pose(1), 0.0, 0.000001); + EXPECT_NEAR(next_pose(2), -M_PI / 2, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model in a curvilinear movement with a + * non-zero orientation + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_1) { + // Arrange + Eigen::Vector3d previous_pose(1, 2, M_PI / 4); + Eigen::Vector4d motion_data(3, 0, M_PI / 16, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 2.9, 0.01); + EXPECT_NEAR(next_pose(1), 4.316, 0.01); + EXPECT_NEAR(next_pose(2), 0.98, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), -2.315, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 1.9, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model in a curvilinear movement with a + * negative orientation + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_2) { + // Arrange + Eigen::Vector3d previous_pose(1, 2, -M_PI / 4); + Eigen::Vector4d motion_data(3, 0, -M_PI / 8, 0); + double delta_t = 1; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 2.656, 0.01); + EXPECT_NEAR(next_pose(1), -0.48, 0.01); + EXPECT_NEAR(next_pose(2), -3 * M_PI / 8, 0.000001); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 2.48, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 1.656, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} + +/** + * @brief Test the constant acceleration and turnrate model in a straight line movement with a + * perpendicular orientation + */ +TEST(CONSTANT_ACCELERATION_TURNRATE_MODEL, CURVILINEAR_MOVEMENT_TEST_3) { + // Arrange + Eigen::Vector3d previous_pose(1, 2, -M_PI / 2); + Eigen::Vector4d motion_data(3, 0, M_PI / 4, 0); + double delta_t = 2; + ConstantAccelerationTurnrateModel model; + + // Act + Eigen::Vector3d next_pose = model.get_next_pose(previous_pose, motion_data, delta_t); + Eigen::Matrix3d pose_jacobian = model.get_jacobian_pose(previous_pose, motion_data, delta_t); + + // Assert + EXPECT_NEAR(next_pose(0), 4.820, 0.01); + EXPECT_NEAR(next_pose(1), -1.820, 0.01); + EXPECT_NEAR(next_pose(2), 0.0, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(0, 0), 1); + EXPECT_FLOAT_EQ(pose_jacobian(0, 1), 0); + EXPECT_NEAR(pose_jacobian(0, 2), 3.820, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(1, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(1, 1), 1); + EXPECT_NEAR(pose_jacobian(1, 2), 3.820, 0.01); + EXPECT_FLOAT_EQ(pose_jacobian(2, 0), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 1), 0); + EXPECT_FLOAT_EQ(pose_jacobian(2, 2), 1); +} \ No newline at end of file diff --git a/src/motion_lib/test/vel_process_model/particle_model_test.cpp b/src/motion_lib/test/vel_process_model/particle_model_test.cpp index 11c714db1..0408a62f7 100644 --- a/src/motion_lib/test/vel_process_model/particle_model_test.cpp +++ b/src/motion_lib/test/vel_process_model/particle_model_test.cpp @@ -41,8 +41,8 @@ TEST(CAParticleModelTest, TestJacobianOfVelocityUpdate) { EXPECT_FLOAT_EQ(jacobian(2, 2), 1.0); EXPECT_FLOAT_EQ(jacobian(0, 1), 0.5); EXPECT_FLOAT_EQ(jacobian(0, 2), 2.0); - EXPECT_FLOAT_EQ(jacobian(1, 0), -0.5); - EXPECT_FLOAT_EQ(jacobian(1, 2), -1.0); + EXPECT_FLOAT_EQ(jacobian(1, 0), 0.5); + EXPECT_FLOAT_EQ(jacobian(1, 2), 1.0); EXPECT_FLOAT_EQ(jacobian(2, 0), 0.0); EXPECT_FLOAT_EQ(jacobian(2, 1), 0.0); } \ No newline at end of file diff --git a/src/perception_sensor_lib/CMakeLists.txt b/src/perception_sensor_lib/CMakeLists.txt index 3a02ac3da..951843aed 100644 --- a/src/perception_sensor_lib/CMakeLists.txt +++ b/src/perception_sensor_lib/CMakeLists.txt @@ -22,6 +22,9 @@ foreach(pkg ${PACKAGES}) find_package(${pkg} REQUIRED) endforeach() +# Find PCL +find_package(PCL REQUIRED COMPONENTS registration common io) + # Enable OpenMP (optional) find_package(OpenMP) if(OpenMP_CXX_FOUND) @@ -31,10 +34,10 @@ endif() set(IMPLEMENTATION_FILES src/data_association/maximum_likelihood_nll.cpp src/data_association/maximum_likelihood_md.cpp + src/data_association/nearest_neighbour_icp.cpp src/data_association/nearest_neighbor.cpp - src/observation_model/slam_observation_model/default_observation_model.cpp - src/observation_model/ve_observation_model/no_slip_bicycle_model.cpp src/data_association/jcbb.cpp + src/observation_model/base_observation_model.cpp src/landmark_filter/consecutive_counter_filter.cpp src/loop_closure/lap_counter.cpp ) @@ -46,10 +49,12 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} ) # Link libraries & dependencies ament_target_dependencies(${PROJECT_NAME} ${PACKAGES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) @@ -73,9 +78,10 @@ if(BUILD_TESTING) set(TESTFILES test/main.cpp - test/observation_model/slam_observation_model/default_observation_model_test.cpp + test/observation_model/base_observation_model_test.cpp test/data_association/maximum_likelihood_nll_test.cpp test/data_association/nearest_neighbor_test.cpp + test/data_association/nearest_neighbour_test.cpp test/data_association/maximum_likelihood_md_test.cpp test/landmark_filter/consecutive_counter_filter_test.cpp ) @@ -89,7 +95,9 @@ if(BUILD_TESTING) $ $ ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} ) + target_link_libraries(${PROJECT_NAME}_test ${PCL_LIBRARIES}) install(TARGETS ${PROJECT_NAME}_test DESTINATION lib/${PROJECT_NAME}) endif() diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/map.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/map.hpp index ed22925bd..e9b6c5d2e 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/map.hpp +++ b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/map.hpp @@ -8,6 +8,7 @@ #include "perception_sensor_lib/data_association/maximum_likelihood_md.hpp" #include "perception_sensor_lib/data_association/maximum_likelihood_nll.hpp" #include "perception_sensor_lib/data_association/nearest_neighbor.hpp" +#include "perception_sensor_lib/data_association/nearest_neighbour_icp.hpp" /** * @brief Map of data association models, with the key being the name of the data association model @@ -27,6 +28,10 @@ const std::map< [](const DataAssociationParameters& params) -> std::shared_ptr { return std::make_shared(params); }}, + {"nearest_neighbour_icp", + [](const DataAssociationParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, {"nearest_neighbor", [](const DataAssociationParameters& params) -> std::shared_ptr { return std::make_shared(params); diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour_icp.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour_icp.hpp new file mode 100644 index 000000000..838680c2a --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/data_association/nearest_neighbour_icp.hpp @@ -0,0 +1,40 @@ +#pragma once +#include +#include +#include +#include + +#include +#include +#include + +#include "common_lib/maths/transformations.hpp" +#include "perception_sensor_lib/data_association/base_data_association.hpp" + +/** + * @brief Data association implementation that uses the Malhanobis Distance only + * as criterion to make observation matches. + * + */ +class NearestNeighbourICP : public DataAssociationModel { + /** + * @brief Uses ICP to transform the observations to the landmarks frame. Used to make ICNN + * reliable to bad pose estimates. + * + * @param landmarks landmarks in the form of [x1, y1, x2, y2, ...] in the global frame + * @param observations observations in the form of [x1, y1, x2, y2, ...] in the global frame + * @return Eigen::VectorXd Transformed observations in the form of [x1, y1, x2, y2, ...] in the + * global frame + */ + Eigen::VectorXd transform_points(const Eigen::VectorXd& landmarks, + const Eigen::VectorXd& observations) const; + +public: + NearestNeighbourICP(const DataAssociationParameters& params) : DataAssociationModel(params) {} + + ~NearestNeighbourICP() = default; + + Eigen::VectorXi associate(const Eigen::VectorXd& landmarks, const Eigen::VectorXd& observations, + const Eigen::MatrixXd& covariance, + const Eigen::VectorXd& observation_confidences) const override; +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp index cab2108f5..0d2c7db67 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp +++ b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/base_landmark_filter.hpp @@ -32,8 +32,9 @@ class LandmarkFilter { * @return Eigen::VectorXd the filtered observations in the form of [x1, y1, x2, y2, ...] in the * global frame */ - virtual Eigen::VectorXd filter(const Eigen::VectorXd& new_observations, - const Eigen::VectorXd& new_observation_confidences) = 0; + virtual Eigen::VectorXd filter(const Eigen::VectorXd& observations, + const Eigen::VectorXd& observation_confidences, + Eigen::VectorXi& associations) = 0; /** * @brief Used by SLAM to signal to the filter that the landmarks are already in SLAM's map, and * they should no longer be returned by the filter as new. diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp index e5553778f..212a646ff 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp +++ b/src/perception_sensor_lib/include/perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp @@ -13,8 +13,22 @@ class ConsecutiveCounterFilter : public LandmarkFilter { std::shared_ptr data_association) : LandmarkFilter(params, data_association), map(Eigen::VectorXd::Zero(0)) {} - Eigen::VectorXd filter(const Eigen::VectorXd& new_observations, - const Eigen::VectorXd& new_observation_confidences) override; + /** + * @brief This function receives a new set of observations and returns a filtered set of landmarks + * in global coordinates that are ready to be added to be added to the map + * @details This filter keeps track of how many consecutive times an observation has been seen. + * If an observation has been seen more than minimum_observation_count_ times consecutively, it is + * added to the map and returned as a new landmark. If an observation is not seen in a given call + * to filter, its counter is reset. + * @param observations Observations in the form of [x1, y1, x2, y2, ...] in the global frame + * @param observation_confidences Confidence in the observations in the same order as the + * observations + * @return Eigen::VectorXd the filtered observations in the form of [x1, y1, x2, y2, ...] in the + * global frame + */ + Eigen::VectorXd filter(const Eigen::VectorXd& observations, + const Eigen::VectorXd& observation_confidences, + Eigen::VectorXi& associations) override; void delete_landmarks(const Eigen::VectorXd& some_landmarks) override; friend class ConsecutiveCounterFilter_TestCase1_Test; diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp index d1c49f408..49621deb6 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp +++ b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp @@ -3,7 +3,7 @@ #include "loop_closure.hpp" /** - * @brief Implementation of LoopClosure that detects when the robot returns near + * @brief Implementation of LoopClosure that detects when the robot returns near * the origin and re‑observes any of the first X cones from the map. */ class LapCounter : public LoopClosure { @@ -26,14 +26,12 @@ class LapCounter : public LoopClosure { * @param observations raw observations (unused in this implementation) * @return {true,0.0} once you re‑see any of map_cones[0..X-1] */ - Result detect( - const Eigen::Vector3d& current_pose, - const Eigen::VectorXi& map_cones, - const Eigen::VectorXi& associations, - const Eigen::VectorXd& observations) const override; + Result detect(const Eigen::Vector3d& current_pose, const Eigen::VectorXd& map_cones, + const Eigen::VectorXi& associations, + const Eigen::VectorXd& observations) const override; private: - double threshold_dist_; + double threshold_dist_; // TODO: make this const or create parameters struct int first_x_cones_; int border_width_; int minimum_confidence_; diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp index e31fb04df..44f790d16 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp +++ b/src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp @@ -2,21 +2,20 @@ #include #include -#include #include +#include /** * @brief Interface for detecting loop closures */ class LoopClosure { public: - /** * @brief Result of loop closure detection * @param detected true if a loop closure was detected * @param offset offset in meters of the map deviation */ - struct Result{ + struct Result { bool detected; double offset; }; @@ -33,9 +32,7 @@ class LoopClosure { * @param observations raw observations * @return result indicating if loop closure was detected */ - virtual Result detect( - const Eigen::Vector3d& current_pose, - const Eigen::VectorXi& map_cones, - const Eigen::VectorXi& associations, - const Eigen::VectorXd& observations) const = 0; + virtual Result detect(const Eigen::Vector3d& current_pose, const Eigen::VectorXd& map_cones, + const Eigen::VectorXi& associations, + const Eigen::VectorXd& observations) const = 0; }; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/slam_observation_model/default_observation_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/base_observation_model.hpp similarity index 100% rename from src/perception_sensor_lib/include/perception_sensor_lib/observation_model/slam_observation_model/default_observation_model.hpp rename to src/perception_sensor_lib/include/perception_sensor_lib/observation_model/base_observation_model.hpp diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/base_observation_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/base_observation_model.hpp deleted file mode 100644 index be2d7643e..000000000 --- a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/base_observation_model.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include -#include - -#include "common_lib/car_parameters/car_parameters.hpp" - -/** - * @brief Base class for velocity estimation observation models. - * - */ -class VEObservationModel { -protected: - std::shared_ptr car_parameters_; - -public: - VEObservationModel(const common_lib::car_parameters::CarParameters& car_parameters) - : car_parameters_( - std::make_shared(car_parameters)) {} - - /** - * @brief Calculates expected observations based on the state. The state vector can contain - * variable information about the vehicle's dynamic state, such as its velocity or acceleration. - * The expected observations are the sensor readings that the model predicts based on the current - * state of the vehicle, and can vary depending on the type of sensor being used. - * - * @param state The state vector containing information about the vehicle's dynamic state. May - * have different sizes for different observation models. - * - * @return Eigen::VectorXd The vector of expected observations which can have different sizes - * depending on the observation model. - */ - virtual Eigen::VectorXd expected_observations(const Eigen::VectorXd& state) const = 0; - - /** - * @brief Calculates the Jacobian of the expected observations with respect to the state. - * - * @param state The state vector containing information about the vehicle's dynamic state. - * - * @return Eigen::MatrixXd The Jacobian matrix of expected observations with respect to the state. - */ - virtual Eigen::MatrixXd expected_observations_jacobian(const Eigen::VectorXd& state) const = 0; -}; diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/map.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/map.hpp deleted file mode 100644 index ad51791df..000000000 --- a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/map.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "no_slip_bicycle_model.hpp" - -/* - * Map of Velocity estimation observation models, with the key being the name of the model and the - * value being a lambda function that returns a shared pointer to the corresponding observation - * model. - */ -const std::map( - const common_lib::car_parameters::CarParameters&)>, - std::less<>> - ve_observation_models_map = { - {"no_slip_bicycle_model", - [](const common_lib::car_parameters::CarParameters& params) - -> std::shared_ptr { - return std::make_shared(params); - }}, -}; diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp deleted file mode 100644 index a301d8a84..000000000 --- a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include "base_observation_model.hpp" - -class NoSlipBicycleModel : public VEObservationModel { -public: - NoSlipBicycleModel(const common_lib::car_parameters::CarParameters& car_parameters) - : VEObservationModel(car_parameters) {} - - /** - * @brief - * - * @param state velocities at the center of mass of the vehicle, in its body frame. - * The state vector must have the following structure: [vx, vy, angular velocity]. - * - * @return Eigen::VectorXd Expected WSS, SAS and Motor Resolver Readings with the following - * structure: [WSS_FL, WSS_FR, WSS_RL, WSS_RR, SAS, Motor Resolver]. - * WSS and Motor resolver readings are in RPMs, SAS is in radians. - */ - Eigen::VectorXd expected_observations(const Eigen::VectorXd& state) const override; - - /** - * @brief calculates the Jacobian of the expected_observations function with respect to the state. - * - * @param state velocities at the CG - * @return Eigen::MatrixXd of size (6, 3) where 6 is the number of expected observations - */ - Eigen::MatrixXd expected_observations_jacobian(const Eigen::VectorXd& state) const override; -}; \ No newline at end of file diff --git a/src/perception_sensor_lib/src/data_association/nearest_neighbour_icp.cpp b/src/perception_sensor_lib/src/data_association/nearest_neighbour_icp.cpp new file mode 100644 index 000000000..01f8883bb --- /dev/null +++ b/src/perception_sensor_lib/src/data_association/nearest_neighbour_icp.cpp @@ -0,0 +1,98 @@ +#include "perception_sensor_lib/data_association/nearest_neighbour_icp.hpp" + +Eigen::VectorXd NearestNeighbourICP::transform_points(const Eigen::VectorXd& landmarks, + const Eigen::VectorXd& observations) const { + const int num_landmarks = landmarks.size() / 2; + const int num_observations = observations.size() / 2; + + // Convert landmarks (reference) to target cloud + pcl::PointCloud::Ptr cloud_target(new pcl::PointCloud()); + cloud_target->reserve(num_landmarks); + for (int i = 0; i < num_landmarks; ++i) { + cloud_target->emplace_back(landmarks(2 * i), landmarks(2 * i + 1), 0.0); + } + + // Convert observations (to be transformed) to source cloud + pcl::PointCloud::Ptr cloud_source(new pcl::PointCloud()); + cloud_source->reserve(num_observations); + for (int i = 0; i < num_observations; ++i) { + cloud_source->emplace_back(observations(2 * i), observations(2 * i + 1), 0.0); + } + + // Run ICP alignment + pcl::IterativeClosestPoint icp; + icp.setInputSource(cloud_source); + icp.setInputTarget(cloud_target); + icp.setMaxCorrespondenceDistance(10); + icp.setMaximumIterations(100); // reasonable upper limit + icp.setTransformationEpsilon(1e-6); + icp.setEuclideanFitnessEpsilon(1e-3); + + pcl::PointCloud aligned_cloud; + icp.align(aligned_cloud); + + // Check if ICP has converged + if (!icp.hasConverged()) { + RCLCPP_WARN(rclcpp::get_logger("NearestNeighbourICP"), "ICP did not converge"); + return observations; // Return original observations if ICP fails + } + + // Get the final transformation + Eigen::Matrix4f T = icp.getFinalTransformation(); + + // Apply T to the observations + Eigen::VectorXd transformed_observations(2 * num_observations); + for (int i = 0; i < num_observations; ++i) { + Eigen::Vector4f p; + p << observations(2 * i), observations(2 * i + 1), 0.0f, 1.0f; + + Eigen::Vector4f p_transformed = T * p; + + transformed_observations(2 * i) = p_transformed(0); + transformed_observations(2 * i + 1) = p_transformed(1); + } + + return transformed_observations; +} + +Eigen::VectorXi NearestNeighbourICP::associate( + const Eigen::VectorXd& landmarks, const Eigen::VectorXd& observations, + const Eigen::MatrixXd& covariance, const Eigen::VectorXd& observation_confidences) const { + const int num_observations = observations.size() / 2; + const int num_landmarks = landmarks.size() / 2; + + Eigen::VectorXi associations = Eigen::VectorXi::Constant(observations.size() / 2, -2); + if (num_observations == 0) { + return associations; + } + Eigen::VectorXd transformed_observations; + + if (num_landmarks > 8) { + transformed_observations = this->transform_points(landmarks, observations); + } else { + transformed_observations = observations; // No transformation for small number of landmarks + } + + for (int i = 0; i < num_observations; i++) { + double min_distance = std::numeric_limits::max(); + int min_index = -1; + for (int j = 0; j < num_landmarks; j++) { + const double euclidean_distance = + std::hypot(transformed_observations(2 * i) - landmarks(2 * j), + transformed_observations(2 * i + 1) - landmarks(2 * j + 1)); + if (euclidean_distance < min_distance) { + min_distance = euclidean_distance; + min_index = 2 * j; + } + } + + if (observation_confidences(i) >= this->_params_.new_landmark_confidence_gate) { + if (min_distance < this->_params_.association_gate) { + associations(i) = min_index; + } else { + associations(i) = -1; + } + } + } + return associations; +} \ No newline at end of file diff --git a/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp b/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp index cf9d32798..ee4f01785 100644 --- a/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp +++ b/src/perception_sensor_lib/src/landmark_filter/consecutive_counter_filter.cpp @@ -1,54 +1,73 @@ #include "perception_sensor_lib/landmark_filter/consecutive_counter_filter.hpp" +#include + #include "common_lib/maths/transformations.hpp" -Eigen::VectorXd ConsecutiveCounterFilter::filter( - const Eigen::VectorXd& new_observations, const Eigen::VectorXd& new_observation_confidences) { +Eigen::VectorXd ConsecutiveCounterFilter::filter(const Eigen::VectorXd& observations, + const Eigen::VectorXd& observation_confidences, + Eigen::VectorXi& associations) { if (this->_params_.minimum_observation_count_ <= 1) { - return new_observations; // No filtering needed + return observations; // No filtering needed + } + + std::vector + filter_to_global_observation_map; // TODO: change to more efficient data structure + + Eigen::VectorXd unfiltered_new_observations; + Eigen::VectorXd unfiltered_new_observations_confidences; + for (int i = 0; i < associations.size(); i++) { + if (associations(i) < 0) { + unfiltered_new_observations.conservativeResize(unfiltered_new_observations.size() + 2); + unfiltered_new_observations(unfiltered_new_observations.size() - 2) = observations(i * 2); + unfiltered_new_observations(unfiltered_new_observations.size() - 1) = observations(i * 2 + 1); + unfiltered_new_observations_confidences.conservativeResize( + unfiltered_new_observations_confidences.size() + 1); + unfiltered_new_observations_confidences(unfiltered_new_observations_confidences.size() - 1) = + observation_confidences(i); + associations(i) = -2; // Mark as not ready to be added to the graph + filter_to_global_observation_map.push_back(i); + } } + // Associate the observations that are considered new with the map stored in this filter Eigen::VectorXi new_associations = this->_data_association_->associate( - this->map, new_observations, {}, new_observation_confidences); + this->map, unfiltered_new_observations, {}, unfiltered_new_observations_confidences); const int num_landmarks = this->map.size() / 2; - const int num_new_observations = new_observations.size() / 2; + const int num_new_observations = unfiltered_new_observations.size() / 2; // Calculate which observations should be added to this filter's map and which landmarks in the // map were observed - std::vector was_observed(num_landmarks, false); + std::vector was_observed_as(num_landmarks, -1); // Map landmark index to observation index Eigen::VectorXd to_be_added_to_map; - for (int i = 0; i < num_new_observations; i++) { - if (new_associations(i) != -1 && new_associations(i) != -2) { - was_observed[new_associations(i) / 2] = true; - } else if (new_associations(i) == -1) { + for (int observation_index = 0; observation_index < num_new_observations; observation_index++) { + if (new_associations(observation_index) != -1 && new_associations(observation_index) != -2) { + was_observed_as[new_associations(observation_index) / 2] = + observation_index; // Map landmark index to observation index + } else if (new_associations(observation_index) == -1) { to_be_added_to_map.conservativeResize(to_be_added_to_map.size() + 2); to_be_added_to_map.segment(to_be_added_to_map.size() - 2, 2) = - new_observations.segment(i * 2, 2); + unfiltered_new_observations.segment(observation_index * 2, 2); } } - // Update counter + // Update counter and set filtered observations Eigen::VectorXd new_map; Eigen::VectorXi new_counter; Eigen::VectorXd filtered_observations; - for (int i = 0; i < num_landmarks; i++) { - if (!was_observed[i]) { + for (int landmark_index = 0; landmark_index < num_landmarks; landmark_index++) { + if (was_observed_as[landmark_index] < 0) { continue; } - if (this->counter(i) >= this->_params_.minimum_observation_count_ - 1) { + if (this->counter(landmark_index) >= this->_params_.minimum_observation_count_ - 1) { filtered_observations.conservativeResize(filtered_observations.size() + 2); - for (int j = 0; j < num_new_observations; j++) { - if (new_associations[j] == i * 2) { - filtered_observations.segment(filtered_observations.size() - 2, 2) = - new_observations.segment(j * 2, 2); - break; - } - } filtered_observations.segment(filtered_observations.size() - 2, 2) = - this->map.segment(i * 2, 2); + unfiltered_new_observations.segment(was_observed_as[landmark_index] * 2, 2); + associations(filter_to_global_observation_map[was_observed_as[landmark_index]]) = + -1; // Mark as ready to be added to the graph } new_map.conservativeResize(new_map.size() + 2); - new_map.segment(new_map.size() - 2, 2) = this->map.segment(i * 2, 2); + new_map.segment(new_map.size() - 2, 2) = this->map.segment(landmark_index * 2, 2); new_counter.conservativeResize(new_counter.size() + 1); - new_counter(new_counter.size() - 1) = this->counter(i) + 1; + new_counter(new_counter.size() - 1) = this->counter(landmark_index) + 1; } new_map.conservativeResize(new_map.size() + to_be_added_to_map.size()); new_map.tail(to_be_added_to_map.size()) = to_be_added_to_map; diff --git a/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp b/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp index ca68890b8..055779106 100644 --- a/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp +++ b/src/perception_sensor_lib/src/loop_closure/lap_counter.cpp @@ -9,8 +9,9 @@ LapCounter::LapCounter(double threshold_dist, int first_x_cones, int border_widt border_width_(border_width), minimum_confidence_(minimum_confidence) {} +// TODO: solve this, not using map_cones or observations LoopClosure::Result LapCounter::detect(const Eigen::Vector3d& current_pose, - const Eigen::VectorXi& map_cones, + const Eigen::VectorXd& map_cones, const Eigen::VectorXi& associations, const Eigen::VectorXd& observations) const { double dx = current_pose.x(); @@ -35,11 +36,8 @@ LoopClosure::Result LapCounter::detect(const Eigen::Vector3d& current_pose, // Look for match with any of the first X cones for (int i = 0; i < associations.size(); ++i) { int j = associations[i]; - if (j >= 0) { - int map_idx = (j) / 2; // Index into map_cones - if (map_idx < first_x_cones_) { - confidence_++; // increase the number of cones that have a match -> increase confidence - } + if (j >= 0 && j / 2 < first_x_cones_) { // If observation macthed with one of the first X cones + confidence_++; // increase the number of cones that have a match -> increase confidence } } diff --git a/src/perception_sensor_lib/src/observation_model/slam_observation_model/default_observation_model.cpp b/src/perception_sensor_lib/src/observation_model/base_observation_model.cpp similarity index 97% rename from src/perception_sensor_lib/src/observation_model/slam_observation_model/default_observation_model.cpp rename to src/perception_sensor_lib/src/observation_model/base_observation_model.cpp index 102d72265..06ea7df8a 100644 --- a/src/perception_sensor_lib/src/observation_model/slam_observation_model/default_observation_model.cpp +++ b/src/perception_sensor_lib/src/observation_model/base_observation_model.cpp @@ -1,4 +1,4 @@ -#include "perception_sensor_lib/observation_model/slam_observation_model/default_observation_model.hpp" +#include "perception_sensor_lib/observation_model/base_observation_model.hpp" #include "common_lib/maths/transformations.hpp" diff --git a/src/perception_sensor_lib/src/observation_model/ve_observation_model/no_slip_bicycle_model.cpp b/src/perception_sensor_lib/src/observation_model/ve_observation_model/no_slip_bicycle_model.cpp deleted file mode 100644 index 16d4e014b..000000000 --- a/src/perception_sensor_lib/src/observation_model/ve_observation_model/no_slip_bicycle_model.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp" - -Eigen::VectorXd NoSlipBicycleModel::expected_observations( - const Eigen::VectorXd& cg_velocities) const { - const double vx = cg_velocities(0); - const double vy = cg_velocities(1); - const double omega = cg_velocities(2); - const double lr = - this->car_parameters_ - ->dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels - const double lf = - this->car_parameters_->wheelbase - - this->car_parameters_ - ->dist_cg_2_rear_axis; // distance from the center of mass to the front wheels - - double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); - double rear_wheels_rpm = - 60 * rear_wheel_velocity / (M_PI * this->car_parameters_->wheel_diameter); - if (vx < 0) { - rear_wheels_rpm = -rear_wheels_rpm; - } - - double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); - double front_wheels_rpm = - 60 * front_wheel_velocity / (M_PI * this->car_parameters_->wheel_diameter); - if (vx < 0) { - front_wheels_rpm = -front_wheels_rpm; - } - double v = sqrt(pow(vx, 2) + pow(vy, 2)); - double steering_angle = - (std::fabs(v) <= 1e-2) ? 0 : atan(omega * (this->car_parameters_->wheelbase) / v); - double motor_rpm = 60 * this->car_parameters_->gear_ratio * rear_wheel_velocity / - (M_PI * this->car_parameters_->wheel_diameter); - if (vx < 0) { - motor_rpm = -motor_rpm; - } - - Eigen::VectorXd observations = Eigen::VectorXd::Zero(6); - observations << front_wheels_rpm, front_wheels_rpm, rear_wheels_rpm, rear_wheels_rpm, - steering_angle, motor_rpm; - return observations; -} - -Eigen::MatrixXd NoSlipBicycleModel::expected_observations_jacobian( - const Eigen::VectorXd& cg_velocities) const { - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 3); - double vx = cg_velocities(0); - double vy = cg_velocities(1); - double omega = cg_velocities(2); - double lr = this->car_parameters_ - ->dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels - double lf = this->car_parameters_->wheelbase - - this->car_parameters_ - ->dist_cg_2_rear_axis; // distance from the center of mass to the front wheels - double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); - double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); - - const double epsilon = 1e-2; - if (std::fabs(rear_wheel_velocity) < epsilon || std::fabs(front_wheel_velocity) < epsilon) { - jacobian(5, 0) = epsilon; - return jacobian; - } - - double sign = (vx < 0) ? -1.0 : 1.0; - jacobian(0, 0) = std::max( - 0.0, sign * 60 * vx / (M_PI * this->car_parameters_->wheel_diameter * front_wheel_velocity)); - jacobian(0, 1) = sign * 60 * (vy + omega * lf) / - (M_PI * this->car_parameters_->wheel_diameter * front_wheel_velocity); - jacobian(0, 2) = sign * 60 * lf * (vy + omega * lf) / - (M_PI * this->car_parameters_->wheel_diameter * front_wheel_velocity); - jacobian(1, 0) = jacobian(0, 0); - jacobian(1, 1) = jacobian(0, 1); - jacobian(1, 2) = jacobian(0, 2); - - jacobian(2, 0) = - sign * 60 * vx / (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); - jacobian(2, 1) = sign * 60 * (vy - omega * lr) / - (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); - jacobian(2, 2) = sign * 60 * (-lr) * (vy - omega * lr) / - (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); - jacobian(3, 0) = jacobian(2, 0); - jacobian(3, 1) = jacobian(2, 1); - jacobian(3, 2) = jacobian(2, 2); - - jacobian(5, 0) = sign * this->car_parameters_->gear_ratio * 60 * vx / - (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); - jacobian(5, 1) = sign * this->car_parameters_->gear_ratio * 60 * (vy - omega * lr) / - (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); - jacobian(5, 2) = sign * this->car_parameters_->gear_ratio * 60 * (-lr) * (vy - omega * lr) / - (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); - double L = this->car_parameters_->wheelbase; - double v = sqrt(pow(vx, 2) + pow(vy, 2)); - if (std::fabs(v) > epsilon) { - jacobian(4, 0) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vx / v); - jacobian(4, 1) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vy / v); - jacobian(4, 2) = (L * v) / (v * v + (omega * L) * (omega * L)); - } - - return jacobian; -} \ No newline at end of file diff --git a/src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp b/src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp new file mode 100644 index 000000000..95184e8d7 --- /dev/null +++ b/src/perception_sensor_lib/test/data_association/nearest_neighbour_test.cpp @@ -0,0 +1,172 @@ +#include + +#include "perception_sensor_lib/data_association/nearest_neighbour_icp.hpp" + +/** + * @brief Test with mostly easy matches and a new landmark. + * + */ +TEST(NearestNeighbourICP, TestCase1) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(10); + observations << 4.5, 13.2, 34.2, -7.2, 3.1, 5.65, 6.86, 9.2, 1.7, 0.5; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbourICP ml(params); + std::vector expected_associations = {-1, 0, 4, 6, 8}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Same case as TestCase1 but with rotation. + * + */ +TEST(NearestNeighbourICP, TestCase2) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(10); + observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975, + 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675, + 1.699999998811323, 0.49999999845647614; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbourICP ml(params); + std::vector expected_associations = {-1, 0, 4, 6, 8}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Same case as TestCase2 but with translation. + * + */ +TEST(NearestNeighbourICP, TestCase3) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(10); + observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025, + 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031, + 1.6999999970212407, 0.49999999653619254; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + std::vector expected_associations = {-1, 0, 4, 6, 8}; + NearestNeighbourICP ml(params); + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Only new landmarks with high confidence + * + */ +TEST(NearestNeighbourICP, TestCase4) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(8); + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbourICP ml(params); + std::vector expected_associations = {-1, -1, -1, -1}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Only new landmarks with low confidence + * + */ +TEST(NearestNeighbourICP, TestCase5) { + // Arrange + Eigen::VectorXd landmarks(10); + landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4; + Eigen::VectorXd observations(8); + observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682, + 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4); + Eigen::MatrixXd covariance(10, 10); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbourICP ml(params); + std::vector expected_associations = {-2, -2, -2, -2}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Only new landmarks with high confidence, high covariance and empty landmarks + * + */ +TEST(NearestNeighbourICP, TestCase6) { + // Arrange + Eigen::VectorXd landmarks(0); + Eigen::VectorXd observations(8); + observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012, + 12.7823521, 0.449790270; + Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbourICP ml(params); + std::vector expected_associations = {-1, -1, -1, -1}; + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + for (int i = 0; i < static_cast(associations.size()); ++i) { + EXPECT_EQ(associations[i], expected_associations[i]); + } +} + +/** + * @brief Empty landmarks and observations + * + */ +TEST(NearestNeighbourICP, TestCase7) { + // Arrange + Eigen::VectorXd landmarks(0); + Eigen::VectorXd observations(0); + Eigen::VectorXd observation_confidences(0); + Eigen::MatrixXd covariance(0, 0); + DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1); + NearestNeighbourICP ml(params); + // Act + Eigen::VectorXi associations = + ml.associate(landmarks, observations, covariance, observation_confidences); + // Assert + EXPECT_EQ(associations.size(), 0); +} diff --git a/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp b/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp index faa9bbeb3..a09c3b849 100644 --- a/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp +++ b/src/perception_sensor_lib/test/landmark_filter/consecutive_counter_filter_test.cpp @@ -13,21 +13,29 @@ TEST(ConsecutiveCounterFilter, TestCase1) { observations << 0.1, 0.2, 0.7, 0.8; Eigen::VectorXd confidences(2); confidences << 0.99, 0.99; + Eigen::VectorXi associations(2); + associations << -1, -1; // Associations are not used in this test LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); - DataAssociationParameters data_association_params(50.0, 0.43, 0.8, 0.1, 0.1); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); std::shared_ptr data_association = std::make_shared(data_association_params); ConsecutiveCounterFilter filter(params, data_association); - Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); - filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); + filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); - filtered_observations = filter.filter(observations, confidences); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); + filtered_observations = filter.filter(observations, confidences, associations); ASSERT_EQ(filtered_observations.size(), 4); EXPECT_EQ(filtered_observations(0), 0.1); EXPECT_EQ(filtered_observations(1), 0.2); EXPECT_EQ(filtered_observations(2), 0.7); EXPECT_EQ(filtered_observations(3), 0.8); + EXPECT_EQ(associations[0], -1); + EXPECT_EQ(associations[1], -1); } /** @@ -38,26 +46,34 @@ TEST(ConsecutiveCounterFilter, TestCase2) { Eigen::VectorXd observations(4); Eigen::VectorXd confidences(2); LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); - DataAssociationParameters data_association_params(50.0, 0.43, 0.8, 0.1, 0.1); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); std::shared_ptr data_association = std::make_shared(data_association_params); ConsecutiveCounterFilter filter(params, data_association); observations << 0.1, 0.2, 0.7, 0.8; confidences << 0.99, 0.99; - Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + Eigen::VectorXi associations(2); + associations << -1, -1; // Associations are not used in this test + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); observations << 0.1, 0.2, 0.7, 0.8; confidences << 0.99, 0.99; - filtered_observations = filter.filter(observations, confidences); + filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); observations << 0.7, 0.8, 0.1, 0.2; confidences << 0.99, 0.99; - filtered_observations = filter.filter(observations, confidences); + filtered_observations = filter.filter(observations, confidences, associations); ASSERT_EQ(filtered_observations.size(), 4); EXPECT_EQ(filtered_observations(0), 0.1); EXPECT_EQ(filtered_observations(1), 0.2); EXPECT_EQ(filtered_observations(2), 0.7); EXPECT_EQ(filtered_observations(3), 0.8); + EXPECT_EQ(associations[0], -1); + EXPECT_EQ(associations[1], -1); } /** @@ -68,22 +84,30 @@ TEST(ConsecutiveCounterFilter, TestCase3) { Eigen::VectorXd observations(4); Eigen::VectorXd confidences(2); LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); - DataAssociationParameters data_association_params(50.0, 0.43, 0.8, 0.1, 0.1); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); std::shared_ptr data_association = std::make_shared(data_association_params); ConsecutiveCounterFilter filter(params, data_association); observations << 0.1, 0.2, 0.7, 0.8; confidences << 0.99, 0.99; - Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + Eigen::VectorXi associations(2); + associations << -1, -1; // Associations are not used in this test + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); observations << 0.1, 0.2, 0.7, 0.8; confidences << 0.99, 0.99; - filtered_observations = filter.filter(observations, confidences); + filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); observations << 0.7, 0.8, 2, 2.1; confidences << 0.99, 0.99; - filtered_observations = filter.filter(observations, confidences); + filtered_observations = filter.filter(observations, confidences, associations); ASSERT_EQ(filtered_observations.size(), 2); + EXPECT_EQ(associations[0], -1); + EXPECT_EQ(associations[1], -2); EXPECT_EQ(filtered_observations(0), 0.7); EXPECT_EQ(filtered_observations(1), 0.8); } @@ -96,22 +120,30 @@ TEST(ConsecutiveCounterFilter, TestCase4) { Eigen::VectorXd observations(4); Eigen::VectorXd confidences(2); LandmarkFilterParameters params = LandmarkFilterParameters(3, 5); - DataAssociationParameters data_association_params(50.0, 0.43, 0.8, 0.1, 0.1); + DataAssociationParameters data_association_params(50.0, 0.43, 0 - 8, 0.1, 0.1); std::shared_ptr data_association = std::make_shared(data_association_params); ConsecutiveCounterFilter filter(params, data_association); observations << 0.1, 0.2, 0.7, 0.8; confidences << 0.99, 0.99; - Eigen::VectorXd filtered_observations = filter.filter(observations, confidences); + Eigen::VectorXi associations(2); + associations << -1, -1; // Associations are not used in this test + Eigen::VectorXd filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); observations << 0.1, 0.2, 0.7, 0.8; confidences << 0.99, 0.99; - filtered_observations = filter.filter(observations, confidences); + filtered_observations = filter.filter(observations, confidences, associations); EXPECT_EQ(filtered_observations.size(), 0); + EXPECT_EQ(associations[0], -2); + EXPECT_EQ(associations[1], -2); observations << 0.6980866604370328, 0.7972940119689251, 2.000155460474267, 2.0952219144232935; confidences << 0.99, 0.99; - filtered_observations = filter.filter(observations, confidences); + filtered_observations = filter.filter(observations, confidences, associations); ASSERT_EQ(filtered_observations.size(), 2); - EXPECT_EQ(filtered_observations(0), 0.7); - EXPECT_EQ(filtered_observations(1), 0.8); + EXPECT_EQ(filtered_observations(0), 0.6980866604370328); + EXPECT_EQ(filtered_observations(1), 0.7972940119689251); + EXPECT_EQ(associations[0], -1); + EXPECT_EQ(associations[1], -2); } \ No newline at end of file diff --git a/src/perception_sensor_lib/test/observation_model/slam_observation_model/default_observation_model_test.cpp b/src/perception_sensor_lib/test/observation_model/base_observation_model_test.cpp similarity index 94% rename from src/perception_sensor_lib/test/observation_model/slam_observation_model/default_observation_model_test.cpp rename to src/perception_sensor_lib/test/observation_model/base_observation_model_test.cpp index 0c724f3f1..c105bd3e9 100644 --- a/src/perception_sensor_lib/test/observation_model/slam_observation_model/default_observation_model_test.cpp +++ b/src/perception_sensor_lib/test/observation_model/base_observation_model_test.cpp @@ -1,9 +1,9 @@ +#include "perception_sensor_lib/observation_model/base_observation_model.hpp" + #include #include -#include "perception_sensor_lib/observation_model/slam_observation_model/default_observation_model.hpp" - /** * @brief Test the observation model with trivial state * diff --git a/src/slam/CMakeLists.txt b/src/slam/CMakeLists.txt index 1c4963bc9..98451bde1 100644 --- a/src/slam/CMakeLists.txt +++ b/src/slam/CMakeLists.txt @@ -13,9 +13,11 @@ set(PACKAGES sensor_msgs custom_interfaces eufs_msgs + nav_msgs std_srvs fs_msgs pacsim + sensor_msgs geometry_msgs common_lib visualization_msgs @@ -23,6 +25,7 @@ set(PACKAGES motion_lib tf2 tf2_ros + tf2_geometry_msgs perception_sensor_lib yaml-cpp ) @@ -48,7 +51,9 @@ set(IMPLEMENTATION_FILES src/slam_solver/slam_solver.cpp src/slam_solver/ekf_slam_solver.cpp src/slam_solver/graph_slam_solver/graph_slam_solver.cpp - src/slam_solver/graph_slam_solver/pose_updater.cpp + src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp + # src/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.cpp + src/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.cpp src/slam_solver/graph_slam_solver/graph_slam_instance.cpp src/slam_solver/graph_slam_solver/utils.cpp src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp diff --git a/src/slam/README.md b/src/slam/README.md index fbd83c88e..8b591544f 100644 --- a/src/slam/README.md +++ b/src/slam/README.md @@ -83,13 +83,120 @@ Below, some diagrams are presented that can illustrate the structure and behavio ### Behaviour +The following diagram illustrates the flow of data through the SLAM node. -### Structure +![Flow Diagram](../../docs/diagrams/slam/flow-diagram.png) -The node is composed by multiple classes. The diagram below illustrates roughly how they sit in the code structure. +### Structure -![Class Diagram](../../docs/diagrams/slam/class.drawio.svg) +The node is composed by multiple classes. The diagram below illustrates roughly how they sit in the code structure and interact with other packages. + +![Class Diagram](../../docs/diagrams/slam/architecture-diagram.png) +![Class Diagram](../../docs/diagrams/slam/graph_slam_structure.png) + +The following are the interfaces for this package: +```cpp +// Subscriptions + if (!params.use_simulated_perception_) { + this->_perception_subscription_ = this->create_subscription( + "/perception/cones", 1, + std::bind(&SLAMNode::_perception_subscription_callback, this, std::placeholders::_1), + subscription_options); + } + if (!params.use_simulated_velocities_) { + this->_velocities_subscription_ = this->create_subscription( + "/state_estimation/velocities", 50, + std::bind(&SLAMNode::_velocities_subscription_callback, this, std::placeholders::_1), + subscription_options); + } + + // Publishers + this->_map_publisher_ = + this->create_publisher("/state_estimation/map", 10); + this->_vehicle_pose_publisher_ = + this->create_publisher("/state_estimation/vehicle_pose", 10); + this->_visualization_map_publisher_ = + this->create_publisher( + "/state_estimation/visualization_map", 10); + this->_associations_visualization_publisher_ = + this->create_publisher( + "/state_estimation/visualization_associations", 10); + this->_trajectory_visualization_publisher_ = + this->create_publisher( + "/state_estimation/visualization/trajectory", 10); + this->_execution_time_publisher_ = this->create_publisher( + "/state_estimation/slam_execution_time", 10); + this->_covariance_publisher_ = this->create_publisher( + "/state_estimation/slam_covariance", 10); + this->_lap_counter_publisher_ = + this->create_publisher("/state_estimation/lap_counter", 10); + this->_tf_broadcaster_ = std::make_shared(this); +``` +for PacSim adapter: +```cpp +if (params.use_simulated_perception_) { + RCLCPP_INFO(this->get_logger(), "Using simulated perception"); + this->_perception_detections_subscription_ = + this->create_subscription( + "/pacsim/perception/lidar/landmarks", 1, + std::bind(&PacsimAdapter::_pacsim_perception_subscription_callback, this, + std::placeholders::_1), + subscription_options); + } + + if (params.use_simulated_velocities_) { + this->_velocities_subscription_ = + this->create_subscription( + "/pacsim/velocity", 1, + std::bind(&PacsimAdapter::_pacsim_velocities_subscription_callback, this, + std::placeholders::_1), + subscription_options); + } + + this->_imu_subscription_ = this->create_subscription( + "/pacsim/imu/cog_imu", 1, + std::bind(&PacsimAdapter::_pacsim_imu_subscription_callback, this, std::placeholders::_1), + subscription_options); + + this->_finished_client_ = this->create_client("/pacsim/finish_signal"); + param_client_ = + this->create_client("/pacsim/pacsim_node/get_parameters"); +``` -The following node shows the interfaces of this node. -![Components Diagram](../../docs/diagrams/slam/components.drawio.svg) \ No newline at end of file +and for vehicle adapter: + +```cpp +_operational_status_subscription_ = + this->create_subscription( + "/vehicle/operational_status", 10, + [this](const custom_interfaces::msg::OperationalStatus::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Operational status received. Mission: %d - Go: %d", + msg->as_mission, msg->go_signal); + _go_ = true; // msg->go_signal; + _mission_ = common_lib::competition_logic::Mission(msg->as_mission); + this->_slam_solver_->set_mission(_mission_); + }); + _finished_client_ = this->create_client("/as_srv/mission_finished"); + // Create a static map frame + _tf_static_broadcaster_ = std::make_shared(*this); + + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = this->get_clock()->now(); + transformStamped.header.frame_id = "map"; // Fixed frame: "map" + transformStamped.child_frame_id = "vehicle_estimate"; // The child frame: "vehicle" + + /// .... + + // LiDAR odometry subscription + if (!params.receive_lidar_odometry_) { + RCLCPP_INFO(this->get_logger(), "Not receiving lidar odometry, using only velocities"); + return; + } + + this->_lidar_odometry_subscription_ = this->create_subscription( + params.lidar_odometry_topic_, 1, + std::bind(&VehicleAdapter::_lidar_odometry_subscription_callback, this, + std::placeholders::_1), + subscription_options); +``` \ No newline at end of file diff --git a/src/slam/dependencies_install.sh b/src/slam/dependencies_install.sh old mode 100755 new mode 100644 diff --git a/src/slam/filter.py b/src/slam/filter.py deleted file mode 100644 index 780b393ea..000000000 --- a/src/slam/filter.py +++ /dev/null @@ -1,50 +0,0 @@ -import re - - -def out_of_square(x1, x2, y1, y2, point_x, point_y): - return point_x < x1 or point_x > x2 or point_y < y1 or point_y > y2 - - -def parse_cones(cones_str): - """Parses a string of cones into a list of tuples.""" - pattern = r"\((-?\d+\.?\d*),\s*(-?\d+\.?\d*)\)" - matches = re.findall(pattern, cones_str) - return [(float(x), float(y)) for x, y in matches] - - -def format_cones(cones): - """Formats a list of tuples into a string of cones.""" - return ", ".join(f"({x}, {y})" for x, y in cones) - - -def filter_cones(cones_str, func): - """Filters cones based on a given function.""" - cones = parse_cones(cones_str) - filtered_cones = [cone for cone in cones if func(cone)] - return format_cones(filtered_cones) - - -# Example function to apply to each cone -def example_function(cone): - x, y = cone - return ( - y < 54.6 - and x > -20 - and y < -x + 85 - and out_of_square(1, 10, -10, -3, x, y) - and out_of_square(0, 17, 4, 15, x, y) - and out_of_square(-10, -8, 34, 36, x, y) - and out_of_square(34, 38, -28, -24, x, y) - and out_of_square(0, 20, -2, 3, x, y) - ) # Example: Keep cones in the first quadrant - - -if __name__ == "__main__": - # Input string of cones - cones_input = "(5.133865045963249, -1.396701762002485), (8.150394376338124, 4.396929969323597), (7.507541529715111, 6.55421565419266), (7.0138673910730995, 7.126181144785143), (1.6594067280773526, 1.8983275415207055), (13.954832424600028, 1.5581381945701365), (5.109304321488534, 1.9307505888394678), (13.939580891057874, -1.765110066320399), (8.777965014771125, 9.207167508105082), (17.336048082575516, 1.7509525045482113), (7.341673985715374, 4.085977161814296), (6.7341480471727495, 11.035330999406892), (17.69898569832601, -1.7187114525920022), (1.2866748571395876, -3.268325090408325), (1.9046559333801265, -5.030924320220947), (8.701283026797377, -6.5442377399450296), (7.785508584377511, -5.858026238328338), (8.499558817500564, 2.3616694690999065), (8.556268542437962, -1.2401039035378258), (1.7106933593750004, -4.648070812225342), (8.515992492799429, 7.9378852066624495), (6.62365200570483, 5.386406808794455), (6.937982407410768, -4.100037945389506), (3.5490245819091797, -6.36557674407959), (7.3786004683902275, 5.078552820271427), (2.0693910121917725, -6.143548488616943), (7.869848477627094, -3.918360129393276), (2.9646904468536377, -9.015334129333496), (10.215872084446476, 12.301624413437533), (2.135297298431397, -6.922608375549316), (9.117102701902088, 2.1574683773703054), (6.242768370541822, -1.182427759834908), (6.375254988765949, 2.219451650728613), (8.820065397317203, 13.756766981367791), (8.845985811589765, -1.1417360421039597), (11.673796482687385, 14.588644256554112), (0.06523650139570236, 4.391510486602783), (9.515140312051573, 4.763438605516055), (1.0190100669860844, 7.838854789733887), (0.6963430285647306, 10.224481471359173), (1.1230174303054818, 9.873774528503418), (3.714826822280883, 10.09099292755127), (2.461215257644654, 10.937617301940918), (6.933457795726114, 11.034366139006938), (2.8608372211456303, 11.418395042419434), (7.237996840364944, 11.875639312445458), (21.389288006241067, 1.6259677215598272), (3.169143023337582, -1.0984654617088352), (21.67373532803474, -1.8342692908291565), (12.41375362496421, -17.701235257685212), (23.80649851294385, 1.9961197784805202), (8.387278698780934, -19.82514652935703), (16.466041046486477, -15.749880953362206), (7.473977412119347, 9.680288587077092), (25.15272092520826, -1.294028009328845), (25.67218001855477, 2.880894551945877), (20.555358426136493, -13.768614097495659), (14.326710954503312, 11.69964504500021), (14.790364276288901, 11.8711901541223), (27.27019783731637, 5.0209375319470855), (28.041716387829606, 0.2878611391680058), (24.56597885677178, -11.75556417741298), (29.346863131841967, 1.6518315205796044), (28.491140585231452, 7.911089016845398), (30.405283878935066, 3.4683791678749563), (18.360601041502743, -18.725990167911974), (26.56613227031285, 11.71889879708702), (28.47078599088699, -9.65345254289306), (21.705684370962377, -17.807561737015618), (28.00318146403647, 10.21948173173525), (24.374923779113043, 14.873201856971892), (31.338837746250817, 5.333937231726506), (26.31917602993796, -14.939705782518173), (14.321481956710834, -20.746608049727364), (16.338061019635973, 13.927701709540445), (32.08211958702466, 7.622306710727417), (31.45766926687491, 11.182466407163043), (30.3792433092678, -12.702264583123213), (32.349674799848344, -7.689711950950968), (29.27917880423643, 14.007778726755369), (27.846587647616893, 18.071618209367053), (16.321321692491505, -14.054197905899159), (36.33817418465225, -5.5577223088373735), (34.51310761863657, -10.72797634049117), (27.269728340573376, 18.67588947367759), (39.22390207049575, -4.059875751166881), (38.382822020111, -8.541536484226961), (41.658263200344514, -2.0592568521214596), (41.68668338052126, -6.59924450802764), (44.89475662479599, 1.595655248295124), (44.57272073175528, -4.324195693744487), (27.093940740889828, 20.856749654672793), (23.511018858327148, 18.57312358602521), (23.991009958088064, 21.65021479003698), (47.73711590444538, -0.6661040100537104), (47.94592914548282, 5.233960954232499), (28.56432352675873, 24.034801223864864), (25.767816608301978, 23.99050600809743), (49.68475805962337, 8.664221623300827), (29.070597441025804, 26.14090555092508), (15.68187215583812, 11.835347200287291), (16.150744399598715, 14.468426001860827), (26.20764813930816, 26.63915068025201), (11.61856488729895, 12.560450607006688), (50.620939739603415, 12.11590187737678), (12.86917078820009, 11.482728713056295), (9.035938132700014, 14.41719044472456), (15.701127408019664, 11.91920204164253), (29.12529000365888, 28.054583962273266), (28.413796456845212, 29.943980377661013), (24.64581836170292, 29.3671324786939), (22.391240507985614, 31.72380336336962), (27.12028355799802, 32.032965884772324), (20.145781556688224, 32.507167010631626), (10.899033171105245, 12.810527640856153), (24.52734685938393, 34.37856741143578), (17.30329357266592, 32.973665946236416), (8.664012598469116, 11.078649232775945), (14.647117870934235, 32.90367216593822), (21.2564885543144, 35.8263024966574), (10.353446223956588, 30.73669689539158), (7.572353485705365, 28.011593699451037), (17.971174893749374, 36.48881312709552), (39.214484996681215, 32.92129544583579), (36.880789292782204, 35.365097768017634), (13.865098340596449, 36.43314365917138), (35.55755622739879, 37.77241547335112), (10.560367840196811, 34.986537613499486), (7.972670697829389, 32.878939322078494), (43.61806833514687, 31.179871424064277), (34.32424589431214, 39.861798049066834), (40.902095534914864, 36.320759307197015), (32.461890807890555, 41.81318335207229), (40.02222103556055, 37.57153216158989), (28.700812455116793, 44.033428725618684), (39.139916439028795, 39.05656122114261), (37.58753907093727, 42.30095582027956), (45.24117491589451, 34.19539021711514), (34.21458313986115, 44.8210719361951), (25.178833335956053, 45.63893945264385), (47.737971866726504, 29.19493942593751), (30.48297865757543, 47.202647731314165), (27.031931465735983, 49.194295133961845), (20.69539070346434, 47.898925347389806), (5.198965806240795, 30.71857352128267), (22.38178083860276, 51.28020264304174), (17.03682359553006, 49.79274041490877), (18.983596034892134, 52.81482728352454), (13.447904245461386, 50.76678169421903), (2.067033862520838, 29.13427574861846), (16.954656978152148, 53.64511528402545), (3.86541500362824, 26.293985347883496), (9.507662469489295, 50.64812414475607), (14.22532623610337, 54.57442513456554), (-1.154290295500072, 27.735417443197505), (4.613555010110206, 50.1512683996718), (0.5912690220888993, 24.67280726942949), (9.111454228796589, 54.202009490352935), (0.48974819327880253, 48.51822376244287), (-4.103946111534365, 27.193762293553963), (-3.098974067645431, 46.502709235671176), (-3.191538790232285, 23.654117493275436), (4.313431310146983, 53.58408495486383), (-6.849335915337426, 44.03194005508362), (16.692776151939604, 14.040560048787096), (-7.2846295351323045, 26.26613653409632), (9.37507462467686, 13.959012648689647), (-7.170722384429113, 22.72439985939919), (-10.300807911105686, 26.386068787584513), (-12.232869396958996, 26.868963374765602), (-10.982405908087513, 22.97519322699486), (-8.573946630523409, 34.29283732787203), (-13.726353980976043, 27.63314577080668), (7.606199112136151, 13.227388977984026), (7.190866066867164, 11.238025806780856), (-12.062027140140508, 38.84414244306795), (-13.434132324522908, 23.519952269127007), (-15.875116677160051, 29.483154478389633), (-9.331095618392784, 41.43955261312478), (-13.501631207792256, 36.25657017803633), (-15.03764680622806, 33.933756488208836), (-16.049453599375866, 31.88012019702476), (-15.50202481462491, 24.645703589334495), (-17.32003764868317, 25.673018538011366), (-19.307190133687367, 28.22486129016517), (-19.572473299892145, 30.428120314107343), (-19.294161259789057, 32.81627609481811), (-14.50147902849382, 41.38844224963543), (-18.26297933722667, 35.449411372148234), (-16.491157060518564, 38.36343825256624), (-11.647212548847197, 44.19968968769584), (-24.74965239100087, 24.042615090361895), (-24.754081373769793, 24.060399449743706), (-24.807976871025133, 24.710831511201707), (-25.229166958246136, 27.186207594398347), (-24.04040668382405, 21.26988176834007), (-23.88039584980375, 19.650121633924257), (-24.54704052418019, 22.921813470159126), (-24.621862741972468, 22.372627926047958), (-30.083910302497035, 25.614472932795465), (-30.95460005144855, 19.568166839661277), (-29.763428316308666, 19.028630973194776), (-24.099714359009372, 20.9926515830559), (-24.24641429542718, 21.656405658994444), (-33.1813678835114, 25.684489228084356), (-23.626658699651443, 17.87454978984368), (-30.084020657226247, 24.507674716232675), (-25.494511913511246, 25.782954639956095), (-24.05715461703155, 20.281185289350283), (-31.14696776440177, 24.278244591562753), (-34.62154485499049, 27.3436999596171), (-34.25058002666488, 22.449821523569884), (-34.58183679212763, 27.296401817012363), (-28.85521425685762, 27.42456499528255), (-8.944542691983884, 47.08762936056697), (-33.18995664402135, 28.24525227987256), (-34.74442727206589, 27.397927584617186), (-32.956422411468694, 27.51150414652451), (-11.765303689099271, 44.21650895668713), (-4.825187560842522, 49.713970275569196), (-0.5268312118643621, 51.94179440642197), (-11.824836138404875, 61.64803922821591), (-13.074259664556008, 60.50502625676922), (-11.75769116198369, 61.680638234652996), (-14.035131287248086, 58.08480957504691), (-17.207818912199976, 61.567688075887325), (-12.13992045974416, 61.507196624240905), (-18.31004485417663, 59.89010685822445), (-13.992618549204517, 60.462037398335525), (-20.040252524834305, 59.816224780698555), (-5.168275081873133, 59.559354673749716), (-3.6473147147790557, 59.71936125615955), (-11.139765159519731, 62.08030013198797), (0.36411439965212866, 64.13313110854206), (-6.855585748884201, 61.283433817407726), (5.815730108807231, 61.579237113752455), (-3.4223448343617977, 61.657426491717196), (-5.228060825847186, 63.71583634429297), (8.107809999352446, 63.363563778038355), (-4.918937284678855, 63.60756581074941), (-7.269312318399131, 63.170190194424045), (1.3761089654662388, 63.40164887061199), (-2.3693209371222395, 69.58956281093444), (-0.6138479969863887, 62.2060365544421), (12.010803885499255, 63.545684726716104), (13.415194373125107, 63.780300650830185), (2.2229770634308905, 67.69546799715783), (1.0011051788603134, 63.70383989628818), (7.322316154475033, 68.00849641148149), (10.338272071089643, 63.75316755596953), (8.155375409528594, 63.1973248353007), (-1.3509451497657499, 62.004254042012114), (12.365415042652323, 63.68303222101902), (-2.0275807810123267, 61.84303707583667), (10.690248654699344, 63.727197598769926), (3.27296153671594, 62.1383552677324), (14.7237911160973, 63.473929611031394), (16.371839846026454, 66.16736489029176), (17.130627778064998, 63.618941263445784), (9.172137093625395, 70.09188246399283), (4.817951350940659, 72.64988070869079), (0.15558632529066937, 66.97893513865009), (16.45364746427244, 64.59753085981382), (19.9130795866411, 62.90064670659457), (21.90112101553024, 64.23712345110026), (2.950299931199463, 72.8566729293821), (5.437508017415144, 71.40082410743473), (16.928716176608006, 63.73184871567589), (6.610810880151912, 68.21939412075733), (7.122934156375438, 70.44441578196205), (25.81334488653737, 62.2289793711972), (23.295162630343093, 61.65493765381369), (18.203289502135174, 65.99307884649929), (21.92302306235335, 61.837992924378895), (20.56016973737321, 65.71715277993009), (22.114797771285932, 63.42447464909701), (20.119239230534323, 62.328794768567015), (12.323097442726919, 64.3793536871934), (29.053840519009917, 59.93132688488009), (25.82334689368173, 62.14985916978153), (17.44538282255066, 63.470511755178876), (18.468847009447412, 64.16061098775792), (18.60830270294003, 63.05768364821974), (23.21485848654005, 63.03755262237431), (21.2146186398873, 62.459937557894385), (19.332713641805352, 62.81740136207013), (21.30725022847026, 65.43862329630292), (25.655195704900596, 63.93968475929602), (32.47613222571047, 59.067514078356446), (24.748478742736594, 71.2607184280351), (28.37565889142019, 65.14529239001641), (25.843759819777524, 61.777817193008914), (33.594181222653326, 58.612457112271066), (35.85404351844386, 58.37561782973586), (30.850933241076607, 60.07213083967107), (31.162865714124642, 66.38324559371792), (37.48857666983119, 58.20380593670826), (35.40154589297177, 59.853905319169144), (30.977057402029267, 66.66553289395678), (40.408887437662116, 56.25600111399828), (37.57736089881333, 56.628798123332736), (42.39818558652728, 58.536242277274944), (44.97185108422949, 54.56454362836075), (49.10267254384972, 49.424433221552185), (49.69609425936593, 50.821843383707844), (46.98810767722776, 50.95198199810688), (48.8865273742701, 49.66921567673891), (50.96702560012701, 45.21125475247331), (49.10522651444543, 48.667755110715426), (48.479119278523285, 49.059324666412216), (48.286130952225676, 52.390401770806065), (48.538890141344055, 52.159443502930124), (43.853410993606815, 56.21724602047844), (42.443562634356766, 58.486777119572245), (49.3663404166257, 32.3185760547772), (43.35023803798019, 56.34639622664968), (53.15840329037428, 44.23375218567182), (50.970478033579134, 51.59969537133373), (49.52769422034311, 27.30359940529099), (51.77586942324316, 50.52129856761573), (51.78225870252513, 29.960035551352533), (53.39025213537315, 48.04422377716467), (54.64527916616737, 42.882182676871125), (55.53597665966326, 36.97865472714537), (51.42957429763884, 25.20563233777781), (53.289200089493875, 44.169485273832215), (54.1851320672781, 39.53525748129252), (55.71020165024287, 38.37537925251937), (54.59114347129899, 35.37880360888193), (54.545022114645946, 26.87978357377446), (59.06952821668677, 38.38817347138983), (60.03795330230355, 39.18709034538995), (52.85502099239155, 22.701238910229296), (58.52245832912942, 39.23299612951126), (56.003335578323416, 24.790311192580084), (54.001235466584404, 18.679249348494384), (53.93422536336323, 19.961937157173615), (56.00913845661823, 35.465234483582), (57.04912372217787, 22.224526030989374), (53.42460081977955, 43.066240393623026), (55.63675307621072, 36.88938806432734), (53.330938024269706, 41.877504055372825), (52.524352377202, 16.307776065147216), (53.52757407425636, 18.219515074482032), (56.41264806205525, 40.15563721003793), (53.646243451108866, 43.24114924268826), (53.77337594531108, 21.070513493925933), (51.57787251978501, 14.494749024092343), (62.16652270486196, 42.213749090909246), (56.83271420992422, 17.613435629755777), (53.28441312967356, 43.733379409960996), (52.14964712043365, 38.865631164505935), (57.670119637375244, 20.041434596769495), (53.93275706879365, 19.949040708839558), (58.213420691018186, 36.468847913478804), (55.81169053366539, 15.247360400761522), (58.23976042373917, 42.47300920099173), (59.33185392504174, 42.4980427938093), (62.54725350618435, 43.03137736955724), (54.893164845903556, 13.045646552545303), (56.601664053149946, 33.521797223616744), (61.983485825449705, 42.4863658080638), (54.08746891787395, 10.946295923727913), (56.42752011473155, 36.32937718889706), (60.44998846490208, 41.26256453164846), (60.65021341861666, 42.704577479019136), (62.282915956592184, 43.44974767235651), (58.53483294267507, 38.58447718178572), (57.68401745573523, 34.87818364117225), (52.88117435522763, 7.22027763272623), (51.09990850274246, 3.2218714717358052), (52.874294592159096, 7.170118999055749), (36.764982681856715, -25.65667195883123), (34.57990648825922, -26.556680596879723), (10.131373799805361, -22.880205825543595), (6.257104828000744, -24.751903209448468), (4.376811055394523, -21.845676416378517), (2.1823272503167015, -26.713408934035602), (0.6510082579812896, -23.534111103174506), (-1.9826467581450464, -28.366699318222942), (-3.5876164681318827, -25.06343240526348), (-7.5999018934218805, -26.36076545388385), (-7.10206702107789, -29.87933170985985), (-11.004265166126034, -26.718004332794496), (-10.952808547999528, -30.280399588741638), (5.985102635345497, -20.021169760578438), (-13.053916903105465, -26.110539542342465), (-12.970625653869757, -29.877290373076903), (-14.536410076026018, -29.27859663583831), (-15.091579614793808, -24.754607772003133), (-15.530706570168343, -22.063720932862847), (-15.42516349643984, -19.066943901199824), (-15.011534449424701, -15.51913462926915), (-16.50187059459203, -28.015809710028496), (-13.881107604531081, -12.093073107314204), (-19.083142163223837, -22.32136148422819), (-18.728281323528762, -24.251186583347533), (-19.013926725973157, -18.692607761873216), (-12.145672002596209, -9.001027291727171), (-18.171787530275047, -26.25245094133307), (-18.58099285726915, -14.788858389019197), (-19.46163923538582, -25.907685663467955), (-1.2511192772237458, -41.17112536801288), (-17.098570901470286, -10.655078727314466), (-15.400039389881137, -7.058873548084523), (-9.841363600539747, -5.7317509649732905), (-12.685220885395752, -3.536559557580313), (-6.901599357884838, -3.254962737017922), (-9.048161831977438, -0.5171521389053673), (-3.932941786010416, -2.0436352666004924), (-5.072080214319033, 1.2535919107821611), (-0.5722046181447417, -1.3587974076191134), (-1.0530786978574391, 1.913300994600631), (2.849396103949815, 2.3963414615030305), (8.73676954184654, -6.098085217279349), (9.017620651895639, 2.316904916204258), (8.677602694142958, -5.815105705791338), (9.847699467491228, 4.305371319197578), (8.616247329210317, 5.69540730143379), (8.174300046586453, 6.850666284017858), (8.503640305028735, -6.106544966837621), (9.046474323383606, 6.3886013434655675), (8.747846131741463, -3.949753875007005), (8.761647123083304, -4.750762568753117), (9.432986815914386, -5.02427533382048), (14.07613112663869, 7.063685080922056)" - - # Apply filter - result = filter_cones(cones_input, example_function) - - # Print result - print(result) diff --git a/src/slam/format.py b/src/slam/format.py deleted file mode 100644 index 64b5a9885..000000000 --- a/src/slam/format.py +++ /dev/null @@ -1,38 +0,0 @@ -import re - -output_string = """track: - version: 1.0 - lanesFirstWithLastConnected: false - start: - pose: [0, 0.0, 0.0] - earthToTrack: - position: [0.0, 0.0, 0.0] - orientation: [0.0, 0.0, 0.0] - left: - right: - time_keeping: - unknown:\n""" - -input_cones = """(21.389288006241067, 1.6259677215598272), (21.67373532803474, -1.8342692908291565), (12.41375362496421, -17.701235257685212), (23.80649851294385, 1.9961197784805202), (8.387278698780934, -19.82514652935703), (16.466041046486477, -15.749880953362206), (25.15272092520826, -1.294028009328845), (25.67218001855477, 2.880894551945877), (20.555358426136493, -13.768614097495659), (27.27019783731637, 5.0209375319470855), (28.041716387829606, 0.2878611391680058), (24.56597885677178, -11.75556417741298), (29.346863131841967, 1.6518315205796044), (28.491140585231452, 7.911089016845398), (30.405283878935066, 3.4683791678749563), (18.360601041502743, -18.725990167911974), (26.56613227031285, 11.71889879708702), (28.47078599088699, -9.65345254289306), (21.705684370962377, -17.807561737015618), (28.00318146403647, 10.21948173173525), (24.374923779113043, 14.873201856971892), (31.338837746250817, 5.333937231726506), (26.31917602993796, -14.939705782518173), (14.321481956710834, -20.746608049727364), (32.08211958702466, 7.622306710727417), (31.45766926687491, 11.182466407163043), (30.3792433092678, -12.702264583123213), (32.349674799848344, -7.689711950950968), (29.27917880423643, 14.007778726755369), (27.846587647616893, 18.071618209367053), (16.321321692491505, -14.054197905899159), (36.33817418465225, -5.5577223088373735), (34.51310761863657, -10.72797634049117), (27.269728340573376, 18.67588947367759), (39.22390207049575, -4.059875751166881), (38.382822020111, -8.541536484226961), (41.658263200344514, -2.0592568521214596), (41.68668338052126, -6.59924450802764), (44.89475662479599, 1.595655248295124), (44.57272073175528, -4.324195693744487), (27.093940740889828, 20.856749654672793), (23.511018858327148, 18.57312358602521), (23.991009958088064, 21.65021479003698), (47.73711590444538, -0.6661040100537104), (47.94592914548282, 5.233960954232499), (28.56432352675873, 24.034801223864864), (25.767816608301978, 23.99050600809743), (49.68475805962337, 8.664221623300827), (29.070597441025804, 26.14090555092508), (26.20764813930816, 26.63915068025201), (50.620939739603415, 12.11590187737678), (29.12529000365888, 28.054583962273266), (28.413796456845212, 29.943980377661013), (24.64581836170292, 29.3671324786939), (22.391240507985614, 31.72380336336962), (27.12028355799802, 32.032965884772324), (20.145781556688224, 32.507167010631626), (24.52734685938393, 34.37856741143578), (17.30329357266592, 32.973665946236416), (14.647117870934235, 32.90367216593822), (21.2564885543144, 35.8263024966574), (10.353446223956588, 30.73669689539158), (7.572353485705365, 28.011593699451037), (17.971174893749374, 36.48881312709552), (39.214484996681215, 32.92129544583579), (36.880789292782204, 35.365097768017634), (13.865098340596449, 36.43314365917138), (35.55755622739879, 37.77241547335112), (10.560367840196811, 34.986537613499486), (7.972670697829389, 32.878939322078494), (43.61806833514687, 31.179871424064277), (34.32424589431214, 39.861798049066834), (40.902095534914864, 36.320759307197015), (32.461890807890555, 41.81318335207229), (40.02222103556055, 37.57153216158989), (28.700812455116793, 44.033428725618684), (39.139916439028795, 39.05656122114261), (37.58753907093727, 42.30095582027956), (45.24117491589451, 34.19539021711514), (34.21458313986115, 44.8210719361951), (25.178833335956053, 45.63893945264385), (47.737971866726504, 29.19493942593751), (30.48297865757543, 47.202647731314165), (27.031931465735983, 49.194295133961845), (20.69539070346434, 47.898925347389806), (5.198965806240795, 30.71857352128267), (22.38178083860276, 51.28020264304174), (17.03682359553006, 49.79274041490877), (18.983596034892134, 52.81482728352454), (13.447904245461386, 50.76678169421903), (2.067033862520838, 29.13427574861846), (16.954656978152148, 53.64511528402545), (3.86541500362824, 26.293985347883496), (9.507662469489295, 50.64812414475607), (14.22532623610337, 54.57442513456554), (-1.154290295500072, 27.735417443197505), (4.613555010110206, 50.1512683996718), (0.5912690220888993, 24.67280726942949), (9.111454228796589, 54.202009490352935), (0.48974819327880253, 48.51822376244287), (-4.103946111534365, 27.193762293553963), (-3.098974067645431, 46.502709235671176), (-3.191538790232285, 23.654117493275436), (4.313431310146983, 53.58408495486383), (-6.849335915337426, 44.03194005508362), (-7.2846295351323045, 26.26613653409632), (-7.170722384429113, 22.72439985939919), (-10.300807911105686, 26.386068787584513), (-12.232869396958996, 26.868963374765602), (-10.982405908087513, 22.97519322699486), (-13.726353980976043, 27.63314577080668), (-12.062027140140508, 38.84414244306795), (-13.434132324522908, 23.519952269127007), (-15.875116677160051, 29.483154478389633), (-9.331095618392784, 41.43955261312478), (-13.501631207792256, 36.25657017803633), (-15.03764680622806, 33.933756488208836), (-16.049453599375866, 31.88012019702476), (-15.50202481462491, 24.645703589334495), (-17.32003764868317, 25.673018538011366), (-19.307190133687367, 28.22486129016517), (-19.572473299892145, 30.428120314107343), (-19.294161259789057, 32.81627609481811), (-14.50147902849382, 41.38844224963543), (-18.26297933722667, 35.449411372148234), (-16.491157060518564, 38.36343825256624), (-11.647212548847197, 44.19968968769584), (-8.944542691983884, 47.08762936056697), (-11.765303689099271, 44.21650895668713), (-4.825187560842522, 49.713970275569196), (-0.5268312118643621, 51.94179440642197), (49.3663404166257, 32.3185760547772), (49.52769422034311, 27.30359940529099), (51.78225870252513, 29.960035551352533), (51.42957429763884, 25.20563233777781), (54.545022114645946, 26.87978357377446), (52.85502099239155, 22.701238910229296), (56.003335578323416, 24.790311192580084), (54.001235466584404, 18.679249348494384), (53.93422536336323, 19.961937157173615), (57.04912372217787, 22.224526030989374), (52.524352377202, 16.307776065147216), (53.52757407425636, 18.219515074482032), (53.77337594531108, 21.070513493925933), (51.57787251978501, 14.494749024092343), (56.83271420992422, 17.613435629755777), (57.670119637375244, 20.041434596769495), (53.93275706879365, 19.949040708839558), (55.81169053366539, 15.247360400761522), (54.893164845903556, 13.045646552545303), (54.08746891787395, 10.946295923727913), (52.88117435522763, 7.22027763272623), (51.09990850274246, 3.2218714717358052), (52.874294592159096, 7.170118999055749), (10.131373799805361, -22.880205825543595), (6.257104828000744, -24.751903209448468), (4.376811055394523, -21.845676416378517), (2.1823272503167015, -26.713408934035602), (0.6510082579812896, -23.534111103174506), (-1.9826467581450464, -28.366699318222942), (-3.5876164681318827, -25.06343240526348), (-7.5999018934218805, -26.36076545388385), (-7.10206702107789, -29.87933170985985), (-11.004265166126034, -26.718004332794496), (-10.952808547999528, -30.280399588741638), (5.985102635345497, -20.021169760578438), (-13.053916903105465, -26.110539542342465), (-12.970625653869757, -29.877290373076903), (-14.536410076026018, -29.27859663583831), (-15.091579614793808, -24.754607772003133), (-15.530706570168343, -22.063720932862847), (-15.42516349643984, -19.066943901199824), (-15.011534449424701, -15.51913462926915), (-16.50187059459203, -28.015809710028496), (-13.881107604531081, -12.093073107314204), (-19.083142163223837, -22.32136148422819), (-18.728281323528762, -24.251186583347533), (-19.013926725973157, -18.692607761873216), (-12.145672002596209, -9.001027291727171), (-18.171787530275047, -26.25245094133307), (-18.58099285726915, -14.788858389019197), (-19.46163923538582, -25.907685663467955), (-1.2511192772237458, -41.17112536801288), (-17.098570901470286, -10.655078727314466), (-15.400039389881137, -7.058873548084523), (-9.841363600539747, -5.7317509649732905), (-12.685220885395752, -3.536559557580313), (-6.901599357884838, -3.254962737017922), (-9.048161831977438, -0.5171521389053673), (-3.932941786010416, -2.0436352666004924), (-5.072080214319033, 1.2535919107821611), (-0.5722046181447417, -1.3587974076191134), (-1.0530786978574391, 1.913300994600631),(5.076833724975586, -1.4492740631103516), (1.6377781629562378, 1.987723469734192), (12.790818214416504, 1.757738471031189), (5.128137111663818, 2.010254144668579), (12.800914764404297, -1.7448102235794067), (16.977638244628906, 1.7025628089904785), (17.379533767700195, -1.7349836826324463), (8.054586410522461, 1.8665956258773804), (7.39292573928833, -1.5718032121658325), (7.704288959503174, 1.9294309616088867), (7.400172710418701, -1.576185703277588), (7.697709083557129, 1.9342889785766602), (7.400914192199707, -1.5711385011672974), (2.10587,-1.63374)""" - -def parse_cones(cones_str): - """Parses a string of cones into a list of tuples.""" - pattern = r"\((-?\d+\.?\d*),\s*(-?\d+\.?\d*)\)" - matches = re.findall(pattern, cones_str) - return [(float(x), float(y)) for x, y in matches] - -def write_cone(cone): - global output_string - x, y = cone - output_string+=""" - position: [""" + str(x) + ", " + str(y) + """, 0.0] - class: unknown\n""" - - -def filter_cones(cones_str): - """Filters cones based on a given function.""" - cones = parse_cones(cones_str) - [cone for cone in cones if write_cone(cone)] - - -filter_cones(input_cones) -print(output_string) diff --git a/src/slam/include/adapter_slam/pacsim.hpp b/src/slam/include/adapter_slam/pacsim.hpp index 8a2dd929d..71cf5e222 100644 --- a/src/slam/include/adapter_slam/pacsim.hpp +++ b/src/slam/include/adapter_slam/pacsim.hpp @@ -7,6 +7,12 @@ #include "pacsim/msg/perception_detections.hpp" #include "ros_node/slam_node.hpp" +/** + * @brief Adapter class to interface with the Pacsim simulator for SLAM + * + * @details This class subscribes to the necessary topics from Pacsim and adapts the data for use in + * the SLAM node + */ class PacsimAdapter : public SLAMNode { rclcpp::Subscription::SharedPtr _perception_detections_subscription_; ///< Subscriber for simulated perception detections @@ -14,6 +20,9 @@ class PacsimAdapter : public SLAMNode { rclcpp::Subscription::SharedPtr _velocities_subscription_; ///< Subscriber for simulated velocities + rclcpp::Subscription::SharedPtr + _imu_subscription_; ///< Subscriber for simulated IMU data + rclcpp::Client::SharedPtr _finished_client_; ///< Client for finished signal @@ -34,6 +43,12 @@ class PacsimAdapter : public SLAMNode { void _pacsim_velocities_subscription_callback( const geometry_msgs::msg::TwistWithCovarianceStamped& msg); + /** + * @brief Callback for simulated IMU data from pacsim + * @param msg Message containing the IMU data of the vehicle + */ + void _pacsim_imu_subscription_callback(const sensor_msgs::msg::Imu& msg); + /** * @brief Fetches the mission from the parameters. */ diff --git a/src/slam/include/adapter_slam/vehicle.hpp b/src/slam/include/adapter_slam/vehicle.hpp index a3244c80e..50c3ca854 100644 --- a/src/slam/include/adapter_slam/vehicle.hpp +++ b/src/slam/include/adapter_slam/vehicle.hpp @@ -1,14 +1,23 @@ #pragma once +#include + #include "custom_interfaces/msg/operational_status.hpp" #include "ros_node/slam_node.hpp" #include "std_srvs/srv/empty.hpp" #include "std_srvs/srv/trigger.hpp" #include "tf2_ros/static_transform_broadcaster.h" +/** + * @brief Adapter class to interface with the vehicle for SLAM + * + * @details This class subscribes to the necessary topics from the vehicle and adapts the data for + * use in the SLAM node + */ class VehicleAdapter : public SLAMNode { rclcpp::Subscription::SharedPtr _operational_status_subscription_; ///< Subscriber for operational status + rclcpp::Subscription::SharedPtr _lidar_odometry_subscription_; rclcpp::Client::SharedPtr _finished_client_; ///< Client for finished signal @@ -20,7 +29,14 @@ class VehicleAdapter : public SLAMNode { /** * @brief Constructor of the vehicle adapter node */ - VehicleAdapter(const SLAMParameters ¶ms); + VehicleAdapter(const SLAMParameters& params); + + /** + * @brief LiDAR odometry subscription callback + * + * @param msg Pose estimated by the LiDAR odometry + */ + void _lidar_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg); /** * @brief Sends the finished signal to the vehicle control diff --git a/src/slam/include/ros_node/slam_node.hpp b/src/slam/include/ros_node/slam_node.hpp index 57d7dbbe8..c0f560ec3 100644 --- a/src/slam/include/ros_node/slam_node.hpp +++ b/src/slam/include/ros_node/slam_node.hpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include @@ -32,6 +34,8 @@ */ class SLAMNode : public rclcpp::Node { protected: + rclcpp::CallbackGroup::SharedPtr _callback_group_; ///< Callback group for subscriptions, used to + ///< control if they are concurrent or not rclcpp::Subscription::SharedPtr _perception_subscription_; rclcpp::Subscription::SharedPtr _velocities_subscription_; @@ -39,32 +43,33 @@ class SLAMNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr _map_publisher_; rclcpp::Publisher::SharedPtr _visualization_map_publisher_; rclcpp::Publisher::SharedPtr - _visualization_perception_map_publisher_; + _trajectory_visualization_publisher_; ///< Publishes full vehicle trajectory rclcpp::Publisher::SharedPtr _associations_visualization_publisher_; - rclcpp::Publisher::SharedPtr _position_publisher_; rclcpp::Publisher::SharedPtr _execution_time_publisher_; - rclcpp::Publisher::SharedPtr _covariance_publisher_; + rclcpp::Publisher::SharedPtr + _covariance_publisher_; ///< Publishes + /// the covariance of the pose rclcpp::Publisher::SharedPtr _lap_counter_publisher_; - rclcpp::Publisher::SharedPtr _perception_delta_publisher_; - rclcpp::Time _last_perception_message_time_ = rclcpp::Time(0); - std::shared_ptr _tf_broadcaster_; - rclcpp::TimerBase::SharedPtr _timer_; /**< timer */ - std::shared_ptr _slam_solver_; /**< SLAM solver object */ + std::shared_ptr + _tf_broadcaster_; ///< Broadcasts map -> vehicle frame transform + rclcpp::TimerBase::SharedPtr _timer_; + std::shared_ptr _slam_solver_; std::vector _perception_map_; common_lib::structures::Velocities _vehicle_state_velocities_; std::vector _track_map_; - Eigen::VectorXi _associations_; /**< Associations of the cones in the map */ + Eigen::VectorXi _associations_; Eigen::VectorXd _observations_global_; /**< Global observations of the cones */ Eigen::VectorXd _map_coordinates_; /**< Coordinates of the cones in the map */ common_lib::structures::Pose _vehicle_pose_; std::shared_ptr> - _execution_times_; //< Execution times: 0 -> total motion; 1 - //-> total observation; the rest are solver specific + _execution_times_; //< Execution times: 0 -> total motion; + // 1 -> total observation; the rest are solver specific common_lib::competition_logic::Mission _mission_ = common_lib::competition_logic::Mission::NONE; bool _go_ = true; /// flag to start the mission std::string _adapter_name_; + std::string _vehicle_frame_id_; ///< Frame id of the vehicle for the transform /** * @brief Callback that updates everytime information @@ -76,22 +81,28 @@ class SLAMNode : public rclcpp::Node { /** * @brief Callback that updates everytime information - * is received from vehicle state estimation node + * is received from velocity estimation node * - * @param msg Message containing the velocitites of the vehicle + * @param msg Message containing the velocities of the vehicle */ void _velocities_subscription_callback(const custom_interfaces::msg::Velocities& msg); + /** + * @brief Callback that updates everytime information + * is received from the IMU + * + * @param msg Message containing the IMU data + */ + void _imu_subscription_callback(const sensor_msgs::msg::Imu& msg); + /** * @brief publishes the localization ('vehicle_pose') to the topic * vehicle_pose - * */ void _publish_vehicle_pose(); /** * @brief publishes the map ('track_map') to the topic track_map - * */ void _publish_map(); diff --git a/src/slam/include/slam_config/general_config.hpp b/src/slam/include/slam_config/general_config.hpp index c5d3bab46..cb695cc02 100644 --- a/src/slam/include/slam_config/general_config.hpp +++ b/src/slam/include/slam_config/general_config.hpp @@ -13,40 +13,57 @@ struct SLAMParameters { bool use_simulated_perception_ = false; bool use_simulated_velocities_ = false; std::string motion_model_name_ = "constant_velocity"; + std::string pose_updater_name_ = "base_pose_updater"; // Name of the pose updater object, + // responsible for keeping pose estimate std::string data_association_model_name_ = "nearest_neighbor"; + std::string lidar_odometry_topic_ = "/fast_limo/state"; // Topic for pose from lidar odometry + bool receive_lidar_odometry_ = false; // Whether to use lidar odometry topic or not std::string slam_solver_name_ = "graph_slam"; std::string landmark_filter_name_ = "consecutive_count"; - std::string frame_id_ = "map"; - float data_association_limit_distance_ = 70; - float observation_x_noise_ = 0.01; - float observation_y_noise_ = 0.01; + std::string frame_id_ = "map"; // Frame for the map + float data_association_limit_distance_ = + 70; // maximum distance to consider a cone for data association + float observation_x_noise_ = + 0.01; // standard deviation of the observation noise in x (range in Graph SLAM) + float observation_y_noise_ = + 0.01; // standard deviation of the observation noise in y (bearing in Graph SLAM) float velocity_x_noise_ = 0.1; float velocity_y_noise_ = 0.1; float angular_velocity_noise_ = 0.1; + float imu_acceleration_x_noise_ = 0.5; double pose_x_initial_noise_ = 0.1; // Initial noise for the pose x double pose_y_initial_noise_ = 0.1; // Initial noise for the pose y double pose_theta_initial_noise_ = 0.1; // Initial noise for the pose theta + double preloaded_map_noise_ = 0.1; // Noise for preloaded map landmarks double data_association_gate_ = 1.23; double new_landmark_confidence_gate_ = 0.6; bool using_preloaded_map_ = false; // Use preloaded map for SLAM double slam_min_pose_difference_ = 0.3; //< Minimum pose difference to add a new pose to the graph double slam_optimization_period_ = 0.0; //< Period for running optimization of the graph (s), - // 0.0 means optimization on observations receival - int minimum_observation_count_ = - 3; // Minimum number of times a landmark must be observed to be added to the map - double minimum_frequency_of_detections_ = - 5; // Minimum frequency of the detections of a landmark to add it to the map + // 0.0 means optimization on observations receival std::string slam_optimization_type_ = "normal_levenberg"; std::string slam_optimization_mode_ = "sync"; - double preloaded_map_noise_ = 0.1; // Noise for preloaded map landmarks + bool slam_optimization_in_poses_ = false; // Whether to optimize when adding poses or not double slam_isam2_relinearize_threshold_ = 0.1; double slam_isam2_relinearize_skip_ = 1; std::string slam_isam2_factorization_ = "QR"; unsigned int sliding_window_size_ = 5; + // Loop closure parameters + double threshold_dist = 4.0; // Distance around origin to trigger loop closure + int first_x_cones = 10; // consider a loop if you see any of these first X cones + int border_width = 5; // distance to givve to start searching for loop closure again + int minimum_confidence = 3; // minimum number of observations to confirm loop closure + + // Perception filter parameters SLAMParameters() = default; + int minimum_observation_count_ = + 3; // Minimum number of times a landmark must be observed to be added to the map + double minimum_frequency_of_detections_ = + 5; // Minimum frequency of the detections of a landmark to add it to the map + SLAMParameters(const SLAMParameters ¶ms); /** @@ -56,18 +73,32 @@ struct SLAMParameters { */ std::string load_config(); + /** + * @brief Get data association parameters + * @return DataAssociationParameters + */ DataAssociationParameters get_data_association_parameters() { return DataAssociationParameters(data_association_limit_distance_, data_association_gate_, new_landmark_confidence_gate_, observation_x_noise_, observation_y_noise_); } + /** + * @brief Overload the output stream operator for SLAMParameters + * @param os Output stream + * @param params SLAMParameters object + * @return std::ostream& Output stream + */ friend std::ostream &operator<<(std::ostream &os, const SLAMParameters ¶ms) { os << "SLAMParameters: {" << ", frame_id_: " << params.frame_id_ << "use_simulated_perception_: " << params.use_simulated_perception_ << ", use_simulated_velocities_: " << params.use_simulated_velocities_ << ", motion_model_name_: " << params.motion_model_name_ + << ", pose_updater_name_: " << params.pose_updater_name_ + << ", landmark_filter_name_: " << params.landmark_filter_name_ + << ", lidar_odometry_topic_: " << params.lidar_odometry_topic_ + << ", receive_lidar_odometry_: " << params.receive_lidar_odometry_ << ", data_association_model_name_: " << params.data_association_model_name_ << ", data_association_limit_distance_: " << params.data_association_limit_distance_ << ", data_association_gate_: " << params.data_association_gate_ @@ -76,6 +107,7 @@ struct SLAMParameters { << ", observation_y_noise_: " << params.observation_y_noise_ << ", velocity_x_noise_: " << params.velocity_x_noise_ << ", velocity_y_noise_: " << params.velocity_y_noise_ + << ", imu_acceleration_x_noise_: " << params.imu_acceleration_x_noise_ << ", pose_x_initial_noise_: " << params.pose_x_initial_noise_ << ", pose_y_initial_noise_: " << params.pose_y_initial_noise_ << ", pose_theta_initial_noise_: " << params.pose_theta_initial_noise_ @@ -85,14 +117,25 @@ struct SLAMParameters { << ", slam_min_pose_difference_: " << params.slam_min_pose_difference_ << ", slam_optimization_mode_: " << params.slam_optimization_mode_ << ", slam_optimization_type_: " << params.slam_optimization_type_ + << ", slam_optimization_in_poses_: " << params.slam_optimization_in_poses_ << ", slam_optimization_period_: " << params.slam_optimization_period_ << ", preloaded_map_noise_: " << params.preloaded_map_noise_ << ", sliding_window_size_: " << params.sliding_window_size_ << ", slam_isam2_relinearize_threshold_: " << params.slam_isam2_relinearize_threshold_ << ", slam_isam2_relinearize_skip_: " << params.slam_isam2_relinearize_skip_ - << ", slam_isam2_factorization_: " << params.slam_isam2_factorization_ << "}"; + << ", slam_isam2_factorization_: " << params.slam_isam2_factorization_ + << ", minimum_observation_count_: " << params.minimum_observation_count_ + << ", minimum_frequency_of_detections_: " << params.minimum_frequency_of_detections_ + << ", threshold_dist: " << params.threshold_dist + << ", first_x_cones: " << params.first_x_cones << ", border_width: " << params.border_width + << ", minimum_confidence: " << params.minimum_confidence << "}"; return os; } + /** + * @brief Overload the assignment operator for SLAMParameters + * @param other SLAMParameters object to assign from + * @return SLAMParameters& Reference to the assigned SLAMParameters object + */ SLAMParameters &operator=(const SLAMParameters &other); }; \ No newline at end of file diff --git a/src/slam/include/slam_solver/ekf_slam_solver.hpp b/src/slam/include/slam_solver/ekf_slam_solver.hpp index b97c2d664..083590f28 100644 --- a/src/slam/include/slam_solver/ekf_slam_solver.hpp +++ b/src/slam/include/slam_solver/ekf_slam_solver.hpp @@ -5,10 +5,11 @@ #include "common_lib/conversions/cones.hpp" #include "common_lib/maths/transformations.hpp" -#include "perception_sensor_lib/observation_model/slam_observation_model/default_observation_model.hpp" +#include "perception_sensor_lib/observation_model/base_observation_model.hpp" #include "slam_solver/slam_solver.hpp" +#include "solver_traits/velocities_integrator_trait.hpp" -class EKFSLAMSolver : public SLAMSolver { +class EKFSLAMSolver : public SLAMSolver, public VelocitiesIntegratorTrait { SLAMParameters slam_parameters_; std::shared_ptr observation_model_; Eigen::VectorXd state_ = Eigen::VectorXd::Zero(3); @@ -105,20 +106,12 @@ class EKFSLAMSolver : public SLAMSolver { std::shared_ptr> execution_times, std::shared_ptr loop_closure); - /** - * @brief Initialize the EKF SLAM solver - * @description This method is used to initialize the EKF SLAM solver's - * aspects that require the node e.g. timer callbacks - * @param node ROS2 node - */ - void init([[maybe_unused]] std::weak_ptr _) override; - /** * @brief Executed to deal with new velocity data * * @param velocities */ - void add_motion_prior(const common_lib::structures::Velocities& velocities) override; + void add_velocities(const common_lib::structures::Velocities& velocities) override; /** * @brief process obervations of landmarks @@ -133,7 +126,7 @@ class EKFSLAMSolver : public SLAMSolver { * * @return Eigen::VectorXd state vector */ - void load_initial_state(const Eigen::VectorXd& map, const Eigen::VectorXd& pose) override; + void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) override; /** * @brief Get the covariance matrix of the EKF diff --git a/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp b/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp index b46bae888..27c5f5173 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/factor_data_structures.hpp @@ -4,22 +4,38 @@ #include #include +/** + * @brief Enum to define the type of motion input + */ +enum class MotionInputType { + VELOCITIES, ///< Vehicle velocities + ODOMETRY ///< Pose difference from odometry +}; + /** * @brief Data structure to hold motion data * @details used to record the velocities received and to redo their processing after optimization */ struct MotionData { - std::shared_ptr velocities_; + std::shared_ptr motion_data_; + std::shared_ptr motion_data_noise_; rclcpp::Time timestamp_; + MotionInputType type_ = MotionInputType::VELOCITIES; ///< Type of motion input MotionData() = default; - MotionData(std::shared_ptr velocities, rclcpp::Time timestamp) - : velocities_(velocities), timestamp_(timestamp) {} + MotionData(std::shared_ptr motion_data, + std::shared_ptr motion_data_noise, rclcpp::Time timestamp, + MotionInputType type = MotionInputType::VELOCITIES) + : motion_data_(motion_data), + motion_data_noise_(motion_data_noise), + timestamp_(timestamp), + type_(type) {} }; /** * @brief Data structure to hold observation data - * @details used to record the velocities received and to redo their processing after optimization + * @details used to record the cone observations received and to redo their processing after + * optimization */ struct ObservationData { std::shared_ptr observations_; diff --git a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp index e61304ff6..26d0b6c2d 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp @@ -20,8 +20,6 @@ class GraphSLAMInstance { _factor_graph_; //< Factor graph for the graph SLAM solver (only factors, no estimates) gtsam::Values _graph_values_; //< Estimate for the graph SLAM solver: l_ are landmarks, x_ are poses - Eigen::Vector3d _accumulated_pose_difference_ = - Eigen::Vector3d::Zero(); //< Accumulated pose difference from the last pose node unsigned int _pose_counter_ = 0; //< Counter for the pose symbols unsigned int _landmark_counter_ = 0; //< Counter for the landmark symbols bool _new_pose_node_ = @@ -43,6 +41,17 @@ class GraphSLAMInstance { ~GraphSLAMInstance() = default; + /** + * @brief Clone the graph SLAM instance + * @details This method is used to create a copy of the graph SLAM instance + * It is useful for polymorphic classes that use pointers + * + * @return A shared pointer to the cloned graph SLAM instance + */ + std::shared_ptr clone() const { + return std::make_shared(*this); + } + /** * @brief Checks if new observation factors should be added * @@ -57,12 +66,23 @@ class GraphSLAMInstance { */ bool new_observation_factors() const; + /** + * @brief Get the number of landmarks + * @return unsigned int number of landmarks + */ unsigned int get_landmark_counter() const; + /** + * @brief Get the number of poses + * @return unsigned int number of poses + */ unsigned int get_pose_counter() const; - // TODO: improve copying times and referencing and stuff like that - + /** + * @brief Get the current pose estimate + * + * @return const gtsam::Pose2 current pose + */ const gtsam::Pose2 get_pose() const; /** @@ -101,19 +121,52 @@ class GraphSLAMInstance { * @param pose Initial pose of the robot in the form of an Eigen vector [x, y, theta] * @param preloaded_map_noise Noise to apply to the preloaded map landmarks */ - void load_initial_state(const Eigen::VectorXd& map, const Eigen::VectorXd& pose, + void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose, double preloaded_map_noise); + /** + * @brief Process a new pose and respective pose difference + * @details This method adds a new pose node to the graph values and calls the + * process_pose_difference method to add the respective pose difference factor to the graph + * @param pose_difference Pose difference from the last pose + * @param noise_vector Noise associated with the pose difference + * @param new_pose The new pose to add to the graph + */ + void process_new_pose(const Eigen::Vector3d& pose_difference, const Eigen::Vector3d& noise_vector, + const Eigen::Vector3d& new_pose); /** - * @brief Add motion prior to the graph - * - * @param pose Pose difference to add to the graph + * @brief Process a pose difference and add the respective factor to the graph + * @param pose_difference Pose difference from the last pose + * @param noise_vector Noise associated with the pose difference + * @param before_pose_id ID of the pose before the difference (optional) + * @param after_pose_id ID of the pose after the difference (optional) + */ + void process_pose_difference(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector, unsigned int before_pose_id, + unsigned int after_pose_id); + + /** + * @brief Process a pose difference and add the respective factor to the graph (after pose is + * current pose) + * @param pose_difference Pose difference from the last pose + * @param noise_vector Noise associated with the pose difference + * @param before_pose_id ID of the pose before the difference (optional) + */ + void process_pose_difference(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector, unsigned int before_pose_id); + + /** + * @brief Process a pose difference and add the respective factor to the graph (using current + * pose and last graphed pose) + * @param pose_difference Pose difference from the last pose + * @param noise_vector Noise associated with the pose difference */ - bool process_pose_difference(const Eigen::Vector3d& pose_difference, - const Eigen::Vector3d& new_pose, bool force_update = false); + void process_pose_difference(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector); /** * @brief Runs optimization on the graph + * @details This method calls the optimizer to run optimization on the current factor graph */ void optimize(); diff --git a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp index ba4895e9c..1cc8d5552 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp @@ -5,14 +5,20 @@ #include #include -#include #include +#include "motion_lib/v2p_models/odometry_model.hpp" +#include "perception_sensor_lib/loop_closure/lap_counter.hpp" #include "slam_solver/graph_slam_solver/factor_data_structures.hpp" #include "slam_solver/graph_slam_solver/graph_slam_instance.hpp" #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp" -#include "slam_solver/graph_slam_solver/pose_updater.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp" #include "slam_solver/slam_solver.hpp" +#include "slam_solver/solver_traits/imu_integrator_trait.hpp" +#include "slam_solver/solver_traits/node_controller_trait.hpp" +#include "slam_solver/solver_traits/odometry_integrator_trait.hpp" +#include "slam_solver/solver_traits/trajectory_calculator.hpp" +#include "slam_solver/solver_traits/velocities_integrator_trait.hpp" /** * @brief Graph SLAM solver class @@ -22,20 +28,29 @@ * This specific class controls the access to the models used in the SLAM implementation, * including measures for parallel execution */ -class GraphSLAMSolver : public SLAMSolver { - GraphSLAMInstance _graph_slam_instance_; //< Instance of the graph SLAM solver - PoseUpdater _pose_updater_; //< Pose updater for the graph SLAM solver +class GraphSLAMSolver : public SLAMSolver, + public VelocitiesIntegratorTrait, + public OdometryIntegratorTrait, + public ImuIntegratorTrait, + public NodeControllerTrait, + public TrajectoryCalculator { + std::shared_ptr _graph_slam_instance_; //< Instance of the graph SLAM solver + std::shared_ptr _pose_updater_; //< Pose updater for the graph SLAM solver + std::shared_ptr + _odometry_model; //< Motion model for odometry integration TODO: improve, I dont like it std::queue _motion_data_queue_; //< Queue of velocities received while optimization ran std::queue _observation_data_queue_; //< Queue of observations received while optimization ran rclcpp::TimerBase::SharedPtr _optimization_timer_; //< Timer for asynchronous optimization - std::shared_mutex _mutex_; //< Mutex for the graph SLAM solver, locks access to the graph + mutable std::shared_mutex + _mutex_; //< Mutex for the graph SLAM solver, locks access to the graph and other vars bool _optimization_under_way_ = false; //< Flag to check if the optimization is under way Eigen::VectorXi _associations_; //< Associations of the cones in the map Eigen::VectorXd _observations_global_; //< Global observations of the cones Eigen::VectorXd _map_coordinates_; //< Coordinates of the landmarks in the map - bool _is_stopped_at_beginning_ = true; + + common_lib::sensor_data::ImuData _last_imu_data_; //< Last IMU data received rclcpp::CallbackGroup::SharedPtr _reentrant_group_; //< Reentrant callback group for the timer callback @@ -48,6 +63,20 @@ class GraphSLAMSolver : public SLAMSolver { */ void _asynchronous_optimization_routine(); + /** + * @brief Adds motion data to the graph if the pose updater is ready + * @details This method checks if the pose updater has a new pose to add to the graph + * If so, it adds the new pose to the graph and resets the pose updater's accumulated pose + * difference + * @param pose_updater Pose updater to get the new pose from + * @param graph_slam_instance Graph SLAM instance to add the new pose to + * @param force_update If true, forces the update even if the pose updater is not ready + * @return true if the pose was added to the graph, false otherwise + */ + bool _add_motion_data_to_graph(const std::shared_ptr pose_updater, + const std::shared_ptr graph_slam_instance, + bool force_update = false); + friend class GraphSlamSolverTest_MotionAndObservation_Test; public: @@ -70,7 +99,7 @@ class GraphSLAMSolver : public SLAMSolver { /** * @brief Initialize the SLAM solver - * @description This method is used to initialize the SLAM solver's + * @details This method is used to initialize the SLAM solver's * aspects that require the node e.g. timer callbacks * * @param node ROS2 node @@ -78,9 +107,27 @@ class GraphSLAMSolver : public SLAMSolver { void init(std::weak_ptr node) override; /** - * @brief Add motion prior to the solver (prediction step) + * @brief Process new velocities data (prediction step) + * @details It calls the pose updater using the motion model to predict the new pose + * and may add the motion data to the graph + * @param velocities Velocities data */ - void add_motion_prior(const common_lib::structures::Velocities& velocities) override; + void add_velocities(const common_lib::structures::Velocities& velocities) override; + + /** + * @brief Add odometry data to the solver (for prediction step) + * @details It calls the pose updater using the motion model to predict the new pose + * and may add the motion data to the graph + * @param pose_difference Pose difference in the form of [dx, dy, dtheta] + */ + void add_odometry(const common_lib::structures::Pose& pose_difference) override; + + /** + * @brief Add IMU data to the solver + * + * @param imu_data IMU data + */ + void add_imu_data(const common_lib::sensor_data::ImuData& imu_data) override; /** * @brief Add observations to the solver (correction step) @@ -96,7 +143,7 @@ class GraphSLAMSolver : public SLAMSolver { * global frame * @param pose Pose of the robot in the form of [x, y, theta] relative to the global frame */ - void load_initial_state(const Eigen::VectorXd& map, const Eigen::VectorXd& pose) override; + void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) override; /** * @brief Get the map estimate object @@ -108,10 +155,17 @@ class GraphSLAMSolver : public SLAMSolver { /** * @brief Get the pose estimate object * - * @ return common_lib::structures::Pose + * @return common_lib::structures::Pose */ common_lib::structures::Pose get_pose_estimate() override; + /** + * @brief Get the trajectory estimate of the car (all poses) + * + * @return std::vector + */ + std::vector get_trajectory_estimate() override; + /** * @brief Get the covariance matrix of the graph * @@ -144,6 +198,9 @@ class GraphSLAMSolver : public SLAMSolver { * - 7: redo time in optimization routine * - 8: total asynchronous optimization routine time * - 9: motion model time - * - 10: factor graph time (in add_motion_prior) + * - 10: factor graph time (in add_velocities) + * - 11: perception filter time + * - 12: loop closure time + * - 13: optimization time in pose update */ }; \ No newline at end of file diff --git a/src/slam/include/slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp b/src/slam/include/slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp index 2839955ad..22cd72e3b 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp @@ -5,12 +5,34 @@ #include "slam_config/general_config.hpp" +/** + * @brief Base class for graph optimizers + * @details This class defines the interface for graph optimizers + * It is used to optimize the factor graph and update the pose and graph values accordingly + */ class BaseOptimizer { protected: const SLAMParameters& _params_; public: explicit BaseOptimizer(const SLAMParameters& params) : _params_(params){}; + BaseOptimizer(const BaseOptimizer& other) : _params_(other._params_){}; + BaseOptimizer& operator=(const BaseOptimizer& other) { + if (this == &other) return *this; // Prevent self-assignment + + // Note: _params_ is a reference, so we do not copy it + return *this; + } + + /** + * @brief Clone the optimizer + * @details This method is used to create a copy of the optimizer + * It is useful for polymorphic classes that use pointers to base class + * + * @return A shared pointer to the cloned optimizer + */ + virtual std::shared_ptr clone() const = 0; + virtual ~BaseOptimizer() = default; /** @@ -21,6 +43,8 @@ class BaseOptimizer { * * @param factor_graph The factor graph to optimize * @param graph_values The values to optimize + * @param pose_num The number of poses in the graph + * @param landmark_num The number of landmarks in the graph * @return The optimized values */ virtual gtsam::Values optimize(gtsam::NonlinearFactorGraph& factor_graph, diff --git a/src/slam/include/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp b/src/slam/include/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp index 3f63799b5..084f6a52d 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp @@ -1,18 +1,58 @@ #pragma once +#include +#include +#include #include +#include +#include #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp" +/** + * @brief ISAM2 optimizer for graph SLAM + * @details This class implements the ISAM2 optimizer for graph SLAM + * It uses the GTSAM ISAM2 implementation to optimize the factor graph incrementally + */ class ISAM2Optimizer : public BaseOptimizer { - gtsam::ISAM2 _isam2_; //< ISAM2 instance for the optimizer - gtsam::Values _last_estimate_; //< Last estimate from the optimizer + gtsam::ISAM2 _isam2_; //< ISAM2 instance for the optimizer + gtsam::Values _last_estimate_; //< Last estimate from the optimizer + gtsam::Values _new_values_; //< New values to add to the optimizer + gtsam::NonlinearFactorGraph _new_factors_; //< New factors to add to the optimizer public: ISAM2Optimizer(const SLAMParameters& params); + ISAM2Optimizer(const ISAM2Optimizer& other); + ISAM2Optimizer& operator=(const ISAM2Optimizer& other); ~ISAM2Optimizer() override = default; - gtsam::Values optimize(gtsam::NonlinearFactorGraph& factor_graph, gtsam::Values& graph_values, + /** + * @brief Clone the optimizer + * @details This method is used to create a copy of the optimizer + * It is useful for polymorphic classes that use pointers to base class + * + * @return A shared pointer to the cloned optimizer + */ + std::shared_ptr clone() const; + + /** + * @brief Optimize the graph + * @details This method is used to run the optimization on the graph + * It also updates the pose and the graph values accordingly afterwards + * It may change the factor graph + * ISAM2 optimization is incremental, so it uses its own graph and values to optimize + * TODO: improve this architecture to avoid confusion + * + * @param factor_graph The factor graph to optimize + * @param graph_values The values to optimize + * @param pose_num The number of poses in the graph + * @param landmark_num The number of landmarks in the graph + * @return The optimized values + */ + gtsam::Values optimize([[maybe_unused]] gtsam::NonlinearFactorGraph& factor_graph, + [[maybe_unused]] gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, [[maybe_unused]] unsigned int landmark_num) override; + + friend class GraphSLAMInstance; }; \ No newline at end of file diff --git a/src/slam/include/slam_solver/graph_slam_solver/optimizer/map.hpp b/src/slam/include/slam_solver/graph_slam_solver/optimizer/map.hpp index f102facdc..28b1a8219 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/optimizer/map.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/optimizer/map.hpp @@ -10,8 +10,8 @@ #include "slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.hpp" /* - * Map of slam_solvers, with the key being the type of the slam_solver and the value being a lambda - * function that returns a shared pointer to the corresponding slam_solver + * Map of optimizers, with the key being the type of the optimizer and the value being a lambda + * function that returns a shared pointer to the corresponding optimizer */ const std::map(const SLAMParameters&)>, std::less<>> diff --git a/src/slam/include/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.hpp b/src/slam/include/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.hpp index ce41ec2f3..3dfc9c904 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.hpp @@ -4,12 +4,38 @@ #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp" +/** + * @brief Normal Levenberg-Marquardt optimizer for graph SLAM + * @details This class implements the normal Levenberg-Marquardt optimizer for graph SLAM + * It uses the GTSAM Levenberg-Marquardt implementation to optimize the factor graph + */ class NormalLevenbergOptimizer : public BaseOptimizer { public: NormalLevenbergOptimizer(const SLAMParameters& params); ~NormalLevenbergOptimizer() override = default; + /** + * @brief Clone the optimizer + * @details This method is used to create a copy of the optimizer + * It is useful for polymorphic classes that use pointers to base class + * + * @return A shared pointer to the cloned optimizer + */ + std::shared_ptr clone() const override; + + /** + * @brief Optimize the graph + * @details This method is used to run the optimization on the graph + * It also updates the pose and the graph values accordingly afterwards + * It may change the factor graph + * + * @param factor_graph The factor graph to optimize + * @param graph_values The values to optimize + * @param pose_num The number of poses in the graph + * @param landmark_num The number of landmarks in the graph + * @return The optimized values + */ gtsam::Values optimize(gtsam::NonlinearFactorGraph& factor_graph, gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, [[maybe_unused]] unsigned int landmark_num) override; diff --git a/src/slam/include/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.hpp b/src/slam/include/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.hpp index f2b30406e..0e332dd9a 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.hpp @@ -4,12 +4,39 @@ #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp" +/** + * @brief Sliding Window Levenberg-Marquardt optimizer for graph SLAM + * @details This class implements the sliding window Levenberg-Marquardt optimizer for graph SLAM + * It uses the GTSAM Levenberg-Marquardt implementation to optimize the factor graph + * within a sliding window of poses + */ class SlidingWindowLevenbergOptimizer : public BaseOptimizer { public: SlidingWindowLevenbergOptimizer(const SLAMParameters& params); ~SlidingWindowLevenbergOptimizer() override = default; + /** + * @brief Clone the optimizer + * @details This method is used to create a copy of the optimizer + * It is useful for polymorphic classes that use pointers to base class + * + * @return A shared pointer to the cloned optimizer + */ + std::shared_ptr clone() const override; + + /** + * @brief Optimize the graph + * @details This method is used to run the optimization on the graph + * It also updates the pose and the graph values accordingly afterwards + * It may change the factor graph + * + * @param factor_graph The factor graph to optimize + * @param graph_values The values to optimize + * @param pose_num The number of poses in the graph + * @param landmark_num The number of landmarks in the graph + * @return The optimized values + */ gtsam::Values optimize(gtsam::NonlinearFactorGraph& factor_graph, gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, [[maybe_unused]] unsigned int landmark_num) override; diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater.hpp deleted file mode 100644 index 02961ed81..000000000 --- a/src/slam/include/slam_solver/graph_slam_solver/pose_updater.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include -#include - -#include "motion_lib/v2p_models/base_v2p_motion_model.hpp" -#include "slam_solver/graph_slam_solver/factor_data_structures.hpp" - -/** - * @brief Class to update the pose of the vehicle - * @details This class is the one to apply the motion model and keep track of the most up to date - * pose - */ -class PoseUpdater { - Eigen::Vector3d _last_pose_; - rclcpp::Time _last_pose_update_ = rclcpp::Time(0); - - bool _received_first_velocities_ = - false; //< Flag to check if the first velocities have been received - -public: - PoseUpdater() = default; - - PoseUpdater(const PoseUpdater& other); - - PoseUpdater& operator=(const PoseUpdater& other); - - /** - * @brief Updates the last pose and returns the pose difference - * - * @param motion_data Motion data containing the velocities and timestamp - * @param motion_model Motion model to apply the velocities - * @return std::pair Pose difference (x, y, theta) from the last - * pose, updated pose - */ - std::pair update_pose( - const MotionData& motion_data, std::shared_ptr motion_model); - - Eigen::Vector3d get_last_pose() const { return _last_pose_; } - - rclcpp::Time get_last_pose_update() const { return _last_pose_update_; } - - void set_last_pose(const Eigen::Vector3d& last_pose) { _last_pose_ = last_pose; } -}; diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp new file mode 100644 index 000000000..4d25bf13f --- /dev/null +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include + +#include "motion_lib/v2p_models/base_v2p_motion_model.hpp" +#include "slam_config/general_config.hpp" +#include "slam_solver/graph_slam_solver/factor_data_structures.hpp" + +/** + * @brief Class to update the pose of the vehicle + * @details This class is the one to apply the motion model and keep track of the most up to date + * pose. It allows for the pose estimate to be calculated independently of the graph optimization, + * and only update the graph when needed. + */ +class PoseUpdater { +protected: + Eigen::Vector3d _last_pose_; ///< Last estimated pose (x, y, theta) + Eigen::Vector3d _last_graphed_pose_ = Eigen::Vector3d::Zero(); ///< Last pose added to the graph + Eigen::Matrix3d _last_pose_covariance_ = + Eigen::Matrix3d::Zero(); ///< Covariance of the pose difference up to the last pose + rclcpp::Time _last_pose_update_ = rclcpp::Time(0); ///< Timestamp of the last pose update + + bool _received_first_motion_data_ = + false; ///< Flag to indicate if the first motion data has been received + bool _new_pose_from_graph_ = false; ///< Flag to indicate if the pose was updated from the graph + +public: + explicit PoseUpdater(const SLAMParameters& params); + + PoseUpdater(const PoseUpdater& other); + + PoseUpdater& operator=(const PoseUpdater& other); + + virtual ~PoseUpdater(); + + /** + * @brief Clone the pose updater + * @details This method is used to create a copy of the pose updater + * It is useful for polymorphic classes that use pointers to base class + * + * @return A shared pointer to the cloned pose updater + */ + virtual std::shared_ptr clone() const; + + /** + * @brief Updates the last pose and returns the pose difference + * + * @param motion_data Motion data containing the velocities and timestamp + * @param motion_model Motion model to apply the velocities + */ + virtual void predict_pose(const MotionData& motion_data, + std::shared_ptr motion_model); + + /** + * @brief Updates the last pose with the given pose + * @details This method is used to set the last pose directly, for example after an optimization + * or when the pose is known from another source. It resets the accumulated pose difference. + * + * @param last_pose The last pose to set + */ + virtual void update_pose(const Eigen::Vector3d& last_pose); + + /** + * @brief Check if the pose is ready for graph update + * @return true if there is a new pose different from the graph, false otherwise + */ + virtual bool pose_ready_for_graph_update() const; + + /** @brief Get the adjoint operator matrix for a given pose + * @details The adjoint operator matrix is used to transform the covariance + * of the pose difference when applying the motion model. It accounts for the + * rotation and translation of the pose. + * @param x_translation The x translation + * @param y_translation The y translation + * @param rotation_angle The rotation angle + * @return The adjoint operator matrix + */ + Eigen::Matrix3d get_adjoint_operator_matrix(const double x_translation, + const double y_translation, + const double rotation_angle) const; + + Eigen::Vector3d get_last_pose() const { return _last_pose_; } + + Eigen::Vector3d get_last_graphed_pose() const { return _last_graphed_pose_; } + + /** + * @brief Get the pose difference noise as standard deviation + * @return The pose difference noise (standard deviation for x, y, theta) + */ + Eigen::Vector3d get_pose_difference_noise() const { + // Return the square root of the diagonal as standard deviation + return _last_pose_covariance_.diagonal().array().sqrt(); + } + + rclcpp::Time get_last_pose_update() const { return _last_pose_update_; } +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp new file mode 100644 index 000000000..e2d8e6dac --- /dev/null +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include + +#include "motion_lib/v2p_models/base_v2p_motion_model.hpp" +#include "slam_config/general_config.hpp" +#include "slam_solver/graph_slam_solver/factor_data_structures.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp" + +/** + * @brief Class to update the pose of the vehicle, including a method to check if the pose is ready + * for graph update depending on the accumulated pose difference + * @details This class is the one to apply the motion model and keep track of the most up to date + * pose. The pose is considered ready for graph update if the accumulated pose difference is greater + * than a minimum threshold + */ +class DifferenceBasedReadyPoseUpdater : public PoseUpdater { + double minimum_pose_difference_ = 0.3; + +public: + explicit DifferenceBasedReadyPoseUpdater(const SLAMParameters& params); + + DifferenceBasedReadyPoseUpdater(const DifferenceBasedReadyPoseUpdater& other); + + DifferenceBasedReadyPoseUpdater& operator=(const DifferenceBasedReadyPoseUpdater& other); + + virtual ~DifferenceBasedReadyPoseUpdater(); + + /** + * @brief Clone the pose updater + * @details This method is used to create a copy of the pose updater + * It is useful for polymorphic classes that use pointers to base class + * + * @return A shared pointer to the cloned pose updater + */ + virtual std::shared_ptr clone() const override; + + /** + * @brief Check if the accumulated pose difference is greater than the minimum threshold + * @return true if the pose is ready for graph update, false otherwise + */ + virtual bool pose_ready_for_graph_update() const override; +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.hpp new file mode 100644 index 000000000..aed10b176 --- /dev/null +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "slam_solver/graph_slam_solver/factor_data_structures.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp" + +/** + * @brief TODO: abandoned for now, not working properly + */ +class DoublePoseUpdater : public DifferenceBasedReadyPoseUpdater, public SecondPoseInputTrait { + bool _received_first_velocities_ = + false; //< Flag to check if the first velocities have been received + bool _received_first_odometry_ = false; //< Flag to check if the first odometry has been received + + rclcpp::Time _last_velocities_receive_time_ = + rclcpp::Time(0); ///< Last pose update timestamp for velocities + + Eigen::Vector3d _accumulated_odometry_pose_difference_ = + Eigen::Vector3d::Zero(); ///< Accumulated odometry pose difference since the last pose update + + Eigen::Vector3d _last_odometry_pose_ = Eigen::Vector3d::Zero(); //< Last odometry pose + +public: + explicit DoublePoseUpdater(const SLAMParameters& params); + + DoublePoseUpdater(const DoublePoseUpdater& other); + + DoublePoseUpdater& operator=(const DoublePoseUpdater& other); + + std::shared_ptr clone() const override { + return std::make_shared(*this); + } + + /** + * @brief Updates the last pose and returns the pose difference + * + * @param motion_data Motion data containing the velocities and timestamp + * @param motion_model Motion model to apply the velocities + */ + void predict_pose(const MotionData& motion_data, + std::shared_ptr motion_model) override; + + Eigen::Vector3d get_second_accumulated_pose_difference() const override; + + bool second_pose_difference_ready() const override; +}; diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/map.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/map.hpp new file mode 100644 index 000000000..13e585b2a --- /dev/null +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/map.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include + +#include "slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp" + +/* + * Map of pose updaters, with the key being the type of the pose updater and the value being a + * lambda function that returns a shared pointer to the corresponding pose updater + */ +const std::map(const SLAMParameters&)>, + std::less<>> + pose_updater_constructors_map = { + {"difference_based_ready", + [](const SLAMParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, + {"base_pose_updater", + [](const SLAMParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp new file mode 100644 index 000000000..52de21fc6 --- /dev/null +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include + +/** + * @brief Trait for pose updaters that keep two different pose difference estimates from different + * sources + * @details This trait defines the interface for pose updaters that keep two different pose + * difference estimates from different sources e.g. keeping one from odometry and one from velocity + * estimation and fusing both through factors in the graph + */ +class SecondPoseInputTrait { + int _last_pose_number_ = 0; + +public: + /** + * @brief Get the accumulated pose difference from the second source + * @return Eigen::Vector3d The accumulated pose difference from the second source + */ + virtual Eigen::Vector3d get_second_accumulated_pose_difference() const = 0; + + /** + * @brief Check if the second pose difference is ready + * @return true if the second pose difference is ready, false otherwise + */ + virtual bool second_pose_difference_ready() const = 0; +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/graph_slam_solver/utils.hpp b/src/slam/include/slam_solver/graph_slam_solver/utils.hpp index 49f0d2b75..c3a9ceb2d 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/utils.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/utils.hpp @@ -6,4 +6,6 @@ gtsam::Pose2 eigen_to_gtsam_pose(const Eigen::Vector3d& pose); -Eigen::Vector3d gtsam_pose_to_eigen(const gtsam::Pose2& pose); \ No newline at end of file +Eigen::Vector3d gtsam_pose_to_eigen(const gtsam::Pose2& pose); + +Eigen::Vector3d pose_difference_eigen(const Eigen::Vector3d& pose1, const Eigen::Vector3d& pose2); \ No newline at end of file diff --git a/src/slam/include/slam_solver/slam_solver.hpp b/src/slam/include/slam_solver/slam_solver.hpp index c4a032d7f..f573cdc38 100644 --- a/src/slam/include/slam_solver/slam_solver.hpp +++ b/src/slam/include/slam_solver/slam_solver.hpp @@ -22,9 +22,9 @@ class SLAMSolver { protected: SLAMParameters _params_; - std::shared_ptr _data_association_; - std::shared_ptr _motion_model_; - std::shared_ptr _landmark_filter_; + std::shared_ptr _data_association_; //< Data association pointer + std::shared_ptr _motion_model_; //< Motion model pointer + std::shared_ptr _landmark_filter_; //< Landmark filter pointer common_lib::competition_logic::Mission _mission_ = common_lib::competition_logic::Mission::NONE; std::shared_ptr> _execution_times_; //< Execution times: 0 -> total motion; 1 -> total @@ -57,22 +57,6 @@ class SLAMSolver { virtual ~SLAMSolver() = default; - /** - * @brief Initialize the SLAM solver - * @description This method is used to initialize the SLAM solver's - * aspects that require the node e.g. timer callbacks - * - * @param node ROS2 node - */ - virtual void init([[maybe_unused]] std::weak_ptr node) = 0; - - /** - * @brief Add motion prior to the solver (prediction step) - * - * @param velocities Velocities of the robot - */ - virtual void add_motion_prior(const common_lib::structures::Velocities& velocities) = 0; - /** * @brief Add observations to the solver (correction step) * @@ -87,7 +71,7 @@ class SLAMSolver { * frame * @param pose initial pose of the robot in the form of [x, y, theta] in the global frame */ - virtual void load_initial_state(const Eigen::VectorXd& map, const Eigen::VectorXd& pose) = 0; + virtual void load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) = 0; /** * @brief Get the map estimate object diff --git a/src/slam/include/slam_solver/solver_traits/imu_integrator_trait.hpp b/src/slam/include/slam_solver/solver_traits/imu_integrator_trait.hpp new file mode 100644 index 000000000..a507e0bd0 --- /dev/null +++ b/src/slam/include/slam_solver/solver_traits/imu_integrator_trait.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "common_lib/sensor_data/imu.hpp" + +/** + * @brief Trait class for IMU integration in SLAM solvers + */ +class ImuIntegratorTrait { +public: + /** + * @brief Integrate IMU data into the SLAM solver + * + * @param pose_difference Pose difference in the form of [dx, dy, dtheta] + */ + virtual void add_imu_data(const common_lib::sensor_data::ImuData &imu_data) = 0; + + virtual ~ImuIntegratorTrait() = default; +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/solver_traits/node_controller_trait.hpp b/src/slam/include/slam_solver/solver_traits/node_controller_trait.hpp new file mode 100644 index 000000000..a2df740db --- /dev/null +++ b/src/slam/include/slam_solver/solver_traits/node_controller_trait.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +/** + * @brief Trait class for node control in SLAM solvers + * @details This trait provides an interface for SLAM solvers to interact with ROS2 nodes, + * allowing them to initialize and manage node-related functionalities. + */ +class NodeControllerTrait { +public: + /** + * @brief Initialize the SLAM solver + * @details This method is used to initialize the SLAM solver's + * aspects that require the node e.g. timer callbacks + * + * @param node ROS2 node + */ + virtual void init(std::weak_ptr node) = 0; + + virtual ~NodeControllerTrait() = default; +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/solver_traits/odometry_integrator_trait.hpp b/src/slam/include/slam_solver/solver_traits/odometry_integrator_trait.hpp new file mode 100644 index 000000000..b6af08efe --- /dev/null +++ b/src/slam/include/slam_solver/solver_traits/odometry_integrator_trait.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "common_lib/structures/pose.hpp" + +/** + * @brief Trait class for odometry integration in SLAM solvers + * @details This trait provides an interface for SLAM solvers to integrate odometry data, + * allowing them to utilize odometry information for improved pose estimation. + */ +class OdometryIntegratorTrait { +public: + /** + * @brief Integrate odometry data into the SLAM solver + * + * @param pose_difference Pose difference in the form of [dx, dy, dtheta] + */ + virtual void add_odometry(const common_lib::structures::Pose& pose_difference) = 0; + + virtual ~OdometryIntegratorTrait() = default; +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/solver_traits/trajectory_calculator.hpp b/src/slam/include/slam_solver/solver_traits/trajectory_calculator.hpp new file mode 100644 index 000000000..677c10498 --- /dev/null +++ b/src/slam/include/slam_solver/solver_traits/trajectory_calculator.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include + +#include "common_lib/structures/pose.hpp" + +/** + * @brief Trait class for trajectory calculation in SLAM solvers + * @details This trait provides an interface for SLAM solvers to calculate and retrieve + * the trajectory estimate based on the integrated sensor data. + */ +class TrajectoryCalculator { +public: + /** + * @brief Get the trajectory estimate object + * + * @return std::vector + */ + virtual std::vector get_trajectory_estimate() = 0; +}; \ No newline at end of file diff --git a/src/slam/include/slam_solver/solver_traits/velocities_integrator_trait.hpp b/src/slam/include/slam_solver/solver_traits/velocities_integrator_trait.hpp new file mode 100644 index 000000000..6f8b31084 --- /dev/null +++ b/src/slam/include/slam_solver/solver_traits/velocities_integrator_trait.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "common_lib/structures/velocities.hpp" + +/** + * @brief Trait class for velocities integration in SLAM solvers + * @details This trait provides an interface for SLAM solvers to integrate + * velocity data, allowing them to utilize motion priors for improved pose estimation. + */ +class VelocitiesIntegratorTrait { +public: + /** + * @brief Add motion prior to the solver (prediction step) + * + * @param velocities Velocities of the robot + */ + virtual void add_velocities(const common_lib::structures::Velocities& velocities) = 0; + + virtual ~VelocitiesIntegratorTrait() = default; +}; \ No newline at end of file diff --git a/src/slam/include/track_loader/track_loader.hpp b/src/slam/include/track_loader/track_loader.hpp index 100af9bb1..f6a1b9cf2 100644 --- a/src/slam/include/track_loader/track_loader.hpp +++ b/src/slam/include/track_loader/track_loader.hpp @@ -53,14 +53,6 @@ void load_acceleration_track(Eigen::Vector3d& start_pose, Eigen::VectorXd& track */ void load_skidpad_track(Eigen::Vector3d& start_pose, Eigen::VectorXd& track); -/** - * @brief loads the trackdrive track from the default path. - * - * @param start_pose vector to fill with the start pose [x, y, theta] - * @param track vector to fill with the track data [x1, y1, x2, y2, ...] - */ -void load_trackdrive_track(Eigen::Vector3d& start_pose, Eigen::VectorXd& track); - /** * @brief Transforms the track as seen from the start pose. * diff --git a/src/slam/maps/trackdrive.yaml b/src/slam/maps/trackdrive.yaml deleted file mode 100644 index 81e4f99d7..000000000 --- a/src/slam/maps/trackdrive.yaml +++ /dev/null @@ -1,403 +0,0 @@ -track: - version: 1.0 - lanesFirstWithLastConnected: false - start: - pose: [0, 0.0, 0.0] - earthToTrack: - position: [0.0, 0.0, 0.0] - orientation: [0.0, 0.0, 0.0] - unknown: - - position: [-3.932941786010416, -2.0436352666004924, 0.0] - class: unknown - - position: [-5.072080214319033, 1.2535919107821611, 0.0] - class: unknown - - position: [-0.5722046181447417, -1.3587974076191134, 0.0] - class: unknown - - position: [-1.0530786978574391, 1.913300994600631, 0.0] - class: unknown - - position: [5.076833724975586, -1.4492740631103516, 0.0] - class: unknown - - position: [1.6377781629562378, 1.987723469734192, 0.0] - class: unknown - - position: [12.790818214416504, 1.757738471031189, 0.0] - class: unknown - - position: [5.128137111663818, 2.010254144668579, 0.0] - class: unknown - - position: [12.800914764404297, -1.7448102235794067, 0.0] - class: unknown - - position: [16.977638244628906, 1.7025628089904785, 0.0] - class: unknown - - position: [17.379533767700195, -1.7349836826324463, 0.0] - class: unknown - - position: [8.054586410522461, 1.8665956258773804, 0.0] - class: unknown - - position: [7.704288959503174, 1.9294309616088867, 0.0] - class: unknown - - position: [7.400172710418701, -1.576185703277588, 0.0] - class: unknown - - position: [2.10587, -1.63374, 0.0] - class: unknown - - position: [21.389288006241067, 1.6259677215598272, 0.0] - class: unknown - - position: [21.67373532803474, -1.8342692908291565, 0.0] - class: unknown - - position: [12.41375362496421, -17.701235257685212, 0.0] - class: unknown - - position: [23.80649851294385, 1.9961197784805202, 0.0] - class: unknown - - position: [8.387278698780934, -19.82514652935703, 0.0] - class: unknown - - position: [16.466041046486477, -15.749880953362206, 0.0] - class: unknown - - position: [25.15272092520826, -1.294028009328845, 0.0] - class: unknown - - position: [25.67218001855477, 2.880894551945877, 0.0] - class: unknown - - position: [20.555358426136493, -13.768614097495659, 0.0] - class: unknown - - position: [27.27019783731637, 5.0209375319470855, 0.0] - class: unknown - - position: [28.041716387829606, 0.2878611391680058, 0.0] - class: unknown - - position: [24.56597885677178, -11.75556417741298, 0.0] - class: unknown - - position: [29.346863131841967, 1.6518315205796044, 0.0] - class: unknown - - position: [28.491140585231452, 7.911089016845398, 0.0] - class: unknown - - position: [30.405283878935066, 3.4683791678749563, 0.0] - class: unknown - - position: [18.360601041502743, -18.725990167911974, 0.0] - class: unknown - - position: [26.56613227031285, 11.71889879708702, 0.0] - class: unknown - - position: [28.47078599088699, -9.65345254289306, 0.0] - class: unknown - - position: [21.705684370962377, -17.807561737015618, 0.0] - class: unknown - - position: [28.00318146403647, 10.21948173173525, 0.0] - class: unknown - - position: [24.374923779113043, 14.873201856971892, 0.0] - class: unknown - - position: [31.338837746250817, 5.333937231726506, 0.0] - class: unknown - - position: [26.31917602993796, -14.939705782518173, 0.0] - class: unknown - - position: [14.321481956710834, -20.746608049727364, 0.0] - class: unknown - - position: [32.08211958702466, 7.622306710727417, 0.0] - class: unknown - - position: [31.45766926687491, 11.182466407163043, 0.0] - class: unknown - - position: [30.3792433092678, -12.702264583123213, 0.0] - class: unknown - - position: [32.349674799848344, -7.689711950950968, 0.0] - class: unknown - - position: [29.27917880423643, 14.007778726755369, 0.0] - class: unknown - - position: [27.846587647616893, 18.071618209367053, 0.0] - class: unknown - - position: [36.33817418465225, -5.5577223088373735, 0.0] - class: unknown - - position: [34.51310761863657, -10.72797634049117, 0.0] - class: unknown - - position: [27.269728340573376, 18.67588947367759, 0.0] - class: unknown - - position: [39.22390207049575, -4.059875751166881, 0.0] - class: unknown - - position: [38.382822020111, -8.541536484226961, 0.0] - class: unknown - - position: [41.658263200344514, -2.0592568521214596, 0.0] - class: unknown - - position: [41.68668338052126, -6.59924450802764, 0.0] - class: unknown - - position: [44.89475662479599, 1.595655248295124, 0.0] - class: unknown - - position: [44.57272073175528, -4.324195693744487, 0.0] - class: unknown - - position: [27.093940740889828, 20.856749654672793, 0.0] - class: unknown - - position: [23.511018858327148, 18.57312358602521, 0.0] - class: unknown - - position: [23.991009958088064, 21.65021479003698, 0.0] - class: unknown - - position: [47.73711590444538, -0.6661040100537104, 0.0] - class: unknown - - position: [47.94592914548282, 5.233960954232499, 0.0] - class: unknown - - position: [28.56432352675873, 24.034801223864864, 0.0] - class: unknown - - position: [25.767816608301978, 23.99050600809743, 0.0] - class: unknown - - position: [49.68475805962337, 8.664221623300827, 0.0] - class: unknown - - position: [29.070597441025804, 26.14090555092508, 0.0] - class: unknown - - position: [26.20764813930816, 26.63915068025201, 0.0] - class: unknown - - position: [50.620939739603415, 12.11590187737678, 0.0] - class: unknown - - position: [29.12529000365888, 28.054583962273266, 0.0] - class: unknown - - position: [28.413796456845212, 29.943980377661013, 0.0] - class: unknown - - position: [24.64581836170292, 29.3671324786939, 0.0] - class: unknown - - position: [22.391240507985614, 31.72380336336962, 0.0] - class: unknown - - position: [27.12028355799802, 32.032965884772324, 0.0] - class: unknown - - position: [20.145781556688224, 32.507167010631626, 0.0] - class: unknown - - position: [24.52734685938393, 34.37856741143578, 0.0] - class: unknown - - position: [17.30329357266592, 32.973665946236416, 0.0] - class: unknown - - position: [14.647117870934235, 32.90367216593822, 0.0] - class: unknown - - position: [21.2564885543144, 35.8263024966574, 0.0] - class: unknown - - position: [10.353446223956588, 30.73669689539158, 0.0] - class: unknown - - position: [7.572353485705365, 28.011593699451037, 0.0] - class: unknown - - position: [17.971174893749374, 36.48881312709552, 0.0] - class: unknown - - position: [39.214484996681215, 32.92129544583579, 0.0] - class: unknown - - position: [36.880789292782204, 35.365097768017634, 0.0] - class: unknown - - position: [13.865098340596449, 36.43314365917138, 0.0] - class: unknown - - position: [35.55755622739879, 37.77241547335112, 0.0] - class: unknown - - position: [10.560367840196811, 34.986537613499486, 0.0] - class: unknown - - position: [7.972670697829389, 32.878939322078494, 0.0] - class: unknown - - position: [43.61806833514687, 31.179871424064277, 0.0] - class: unknown - - position: [34.32424589431214, 39.861798049066834, 0.0] - class: unknown - - position: [40.902095534914864, 36.320759307197015, 0.0] - class: unknown - - position: [32.461890807890555, 41.81318335207229, 0.0] - class: unknown - - position: [40.02222103556055, 37.57153216158989, 0.0] - class: unknown - - position: [28.700812455116793, 44.033428725618684, 0.0] - class: unknown - - position: [39.139916439028795, 39.05656122114261, 0.0] - class: unknown - - position: [37.58753907093727, 42.30095582027956, 0.0] - class: unknown - - position: [45.24117491589451, 34.19539021711514, 0.0] - class: unknown - - position: [34.21458313986115, 44.8210719361951, 0.0] - class: unknown - - position: [25.178833335956053, 45.63893945264385, 0.0] - class: unknown - - position: [47.737971866726504, 29.19493942593751, 0.0] - class: unknown - - position: [30.48297865757543, 47.202647731314165, 0.0] - class: unknown - - position: [27.031931465735983, 49.194295133961845, 0.0] - class: unknown - - position: [20.69539070346434, 47.898925347389806, 0.0] - class: unknown - - position: [5.198965806240795, 30.71857352128267, 0.0] - class: unknown - - position: [22.38178083860276, 51.28020264304174, 0.0] - class: unknown - - position: [17.03682359553006, 49.79274041490877, 0.0] - class: unknown - - position: [18.983596034892134, 52.81482728352454, 0.0] - class: unknown - - position: [13.447904245461386, 50.76678169421903, 0.0] - class: unknown - - position: [2.067033862520838, 29.13427574861846, 0.0] - class: unknown - - position: [16.954656978152148, 53.64511528402545, 0.0] - class: unknown - - position: [3.86541500362824, 26.293985347883496, 0.0] - class: unknown - - position: [9.507662469489295, 50.64812414475607, 0.0] - class: unknown - - position: [14.22532623610337, 54.57442513456554, 0.0] - class: unknown - - position: [-1.154290295500072, 27.735417443197505, 0.0] - class: unknown - - position: [4.613555010110206, 50.1512683996718, 0.0] - class: unknown - - position: [0.5912690220888993, 24.67280726942949, 0.0] - class: unknown - - position: [9.111454228796589, 54.202009490352935, 0.0] - class: unknown - - position: [0.48974819327880253, 48.51822376244287, 0.0] - class: unknown - - position: [-4.103946111534365, 27.193762293553963, 0.0] - class: unknown - - position: [-3.098974067645431, 46.502709235671176, 0.0] - class: unknown - - position: [-3.191538790232285, 23.654117493275436, 0.0] - class: unknown - - position: [4.313431310146983, 53.58408495486383, 0.0] - class: unknown - - position: [-6.849335915337426, 44.03194005508362, 0.0] - class: unknown - - position: [-7.2846295351323045, 26.26613653409632, 0.0] - class: unknown - - position: [-7.170722384429113, 22.72439985939919, 0.0] - class: unknown - - position: [-10.300807911105686, 26.386068787584513, 0.0] - class: unknown - - position: [-12.232869396958996, 26.868963374765602, 0.0] - class: unknown - - position: [-10.982405908087513, 22.97519322699486, 0.0] - class: unknown - - position: [-13.726353980976043, 27.63314577080668, 0.0] - class: unknown - - position: [-12.062027140140508, 38.84414244306795, 0.0] - class: unknown - - position: [-13.434132324522908, 23.519952269127007, 0.0] - class: unknown - - position: [-15.875116677160051, 29.483154478389633, 0.0] - class: unknown - - position: [-9.331095618392784, 41.43955261312478, 0.0] - class: unknown - - position: [-13.501631207792256, 36.25657017803633, 0.0] - class: unknown - - position: [-15.03764680622806, 33.933756488208836, 0.0] - class: unknown - - position: [-16.049453599375866, 31.88012019702476, 0.0] - class: unknown - - position: [-15.50202481462491, 24.645703589334495, 0.0] - class: unknown - - position: [-17.32003764868317, 25.673018538011366, 0.0] - class: unknown - - position: [-19.307190133687367, 28.22486129016517, 0.0] - class: unknown - - position: [-19.572473299892145, 30.428120314107343, 0.0] - class: unknown - - position: [-19.294161259789057, 32.81627609481811, 0.0] - class: unknown - - position: [-14.50147902849382, 41.38844224963543, 0.0] - class: unknown - - position: [-18.26297933722667, 35.449411372148234, 0.0] - class: unknown - - position: [-16.491157060518564, 38.36343825256624, 0.0] - class: unknown - - position: [-11.647212548847197, 44.19968968769584, 0.0] - class: unknown - - position: [-8.944542691983884, 47.08762936056697, 0.0] - class: unknown - - position: [-4.825187560842522, 49.713970275569196, 0.0] - class: unknown - - position: [-0.5268312118643621, 51.94179440642197, 0.0] - class: unknown - - position: [49.3663404166257, 32.3185760547772, 0.0] - class: unknown - - position: [49.52769422034311, 27.30359940529099, 0.0] - class: unknown - - position: [51.78225870252513, 29.960035551352533, 0.0] - class: unknown - - position: [51.42957429763884, 25.20563233777781, 0.0] - class: unknown - - position: [54.545022114645946, 26.87978357377446, 0.0] - class: unknown - - position: [52.85502099239155, 22.701238910229296, 0.0] - class: unknown - - position: [56.003335578323416, 24.790311192580084, 0.0] - class: unknown - - position: [54.001235466584404, 18.679249348494384, 0.0] - class: unknown - - position: [57.04912372217787, 22.224526030989374, 0.0] - class: unknown - - position: [52.524352377202, 16.307776065147216, 0.0] - class: unknown - - position: [53.52757407425636, 18.219515074482032, 0.0] - class: unknown - - position: [53.77337594531108, 21.070513493925933, 0.0] - class: unknown - - position: [51.57787251978501, 14.494749024092343, 0.0] - class: unknown - - position: [56.83271420992422, 17.613435629755777, 0.0] - class: unknown - - position: [57.670119637375244, 20.041434596769495, 0.0] - class: unknown - - position: [53.93275706879365, 19.949040708839558, 0.0] - class: unknown - - position: [55.81169053366539, 15.247360400761522, 0.0] - class: unknown - - position: [54.893164845903556, 13.045646552545303, 0.0] - class: unknown - - position: [54.08746891787395, 10.946295923727913, 0.0] - class: unknown - - position: [51.09990850274246, 3.2218714717358052, 0.0] - class: unknown - - position: [52.874294592159096, 7.170118999055749, 0.0] - class: unknown - - position: [10.131373799805361, -22.880205825543595, 0.0] - class: unknown - - position: [6.257104828000744, -24.751903209448468, 0.0] - class: unknown - - position: [4.376811055394523, -21.845676416378517, 0.0] - class: unknown - - position: [2.1823272503167015, -26.713408934035602, 0.0] - class: unknown - - position: [0.6510082579812896, -23.534111103174506, 0.0] - class: unknown - - position: [-1.9826467581450464, -28.366699318222942, 0.0] - class: unknown - - position: [-3.5876164681318827, -25.06343240526348, 0.0] - class: unknown - - position: [-7.5999018934218805, -26.36076545388385, 0.0] - class: unknown - - position: [-7.10206702107789, -29.87933170985985, 0.0] - class: unknown - - position: [-11.004265166126034, -26.718004332794496, 0.0] - class: unknown - - position: [-10.952808547999528, -30.280399588741638, 0.0] - class: unknown - - position: [-13.053916903105465, -26.110539542342465, 0.0] - class: unknown - - position: [-12.970625653869757, -29.877290373076903, 0.0] - class: unknown - - position: [-14.536410076026018, -29.27859663583831, 0.0] - class: unknown - - position: [-15.091579614793808, -24.754607772003133, 0.0] - class: unknown - - position: [-15.530706570168343, -22.063720932862847, 0.0] - class: unknown - - position: [-15.42516349643984, -19.066943901199824, 0.0] - class: unknown - - position: [-15.011534449424701, -15.51913462926915, 0.0] - class: unknown - - position: [-16.50187059459203, -28.015809710028496, 0.0] - class: unknown - - position: [-13.881107604531081, -12.093073107314204, 0.0] - class: unknown - - position: [-19.083142163223837, -22.32136148422819, 0.0] - class: unknown - - position: [-18.728281323528762, -24.251186583347533, 0.0] - class: unknown - - position: [-19.013926725973157, -18.692607761873216, 0.0] - class: unknown - - position: [-12.145672002596209, -9.001027291727171, 0.0] - class: unknown - - position: [-18.171787530275047, -26.25245094133307, 0.0] - class: unknown - - position: [-18.58099285726915, -14.788858389019197, 0.0] - class: unknown - - position: [-17.098570901470286, -10.655078727314466, 0.0] - class: unknown - - position: [-15.400039389881137, -7.058873548084523, 0.0] - class: unknown - - position: [-9.841363600539747, -5.7317509649732905, 0.0] - class: unknown - - position: [-12.685220885395752, -3.536559557580313, 0.0] - class: unknown - - position: [-6.901599357884838, -3.254962737017922, 0.0] - class: unknown - - position: [-9.048161831977438, -0.5171521389053673, 0.0] - class: unknown \ No newline at end of file diff --git a/src/slam/package.xml b/src/slam/package.xml index 08d95859d..c9abb3f1a 100644 --- a/src/slam/package.xml +++ b/src/slam/package.xml @@ -16,11 +16,14 @@ message_filters pacsim std_srvs + nav_msgs + sensor_msgs visualization_msgs motion_lib perception_sensor_lib tf2 tf2_ros + tf2_geometry_msgs ament_lint_auto ament_cmake_gmock diff --git a/src/slam/printer.py b/src/slam/printer.py deleted file mode 100644 index 3f183b87f..000000000 --- a/src/slam/printer.py +++ /dev/null @@ -1,37 +0,0 @@ -import rclpy -from rclpy.node import Node -from custom_interfaces.msg import ConeArray - - -class MapPrinterNode(Node): - def __init__(self): - super().__init__("map_printer_node") - self.subscription = self.create_subscription( - ConeArray, "/state_estimation/map", self.map_callback, 10 - ) - self.get_logger().info("MapPrinterNode has been started.") - - def map_callback(self, msg): - cones = msg.cone_array - - formatted_cones = ", ".join( - f"({cones[i].position.x}, {cones[i].position.y})" - for i in range(0, len(cones)) - ) - self.get_logger().info(f"Cones: {formatted_cones}") - - -def main(args=None): - rclpy.init(args=args) - node = MapPrinterNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/slam/src/adapter_slam/pacsim.cpp b/src/slam/src/adapter_slam/pacsim.cpp index ba1d1174f..3237ff263 100644 --- a/src/slam/src/adapter_slam/pacsim.cpp +++ b/src/slam/src/adapter_slam/pacsim.cpp @@ -8,12 +8,16 @@ #include "ros_node/slam_node.hpp" PacsimAdapter::PacsimAdapter(const SLAMParameters& params) : SLAMNode(params) { + rclcpp::SubscriptionOptions subscription_options; + subscription_options.callback_group = this->_callback_group_; if (params.use_simulated_perception_) { + RCLCPP_INFO(this->get_logger(), "Using simulated perception"); this->_perception_detections_subscription_ = this->create_subscription( "/pacsim/perception/lidar/landmarks", 1, std::bind(&PacsimAdapter::_pacsim_perception_subscription_callback, this, - std::placeholders::_1)); + std::placeholders::_1), + subscription_options); } if (params.use_simulated_velocities_) { @@ -21,9 +25,15 @@ PacsimAdapter::PacsimAdapter(const SLAMParameters& params) : SLAMNode(params) { this->create_subscription( "/pacsim/velocity", 1, std::bind(&PacsimAdapter::_pacsim_velocities_subscription_callback, this, - std::placeholders::_1)); + std::placeholders::_1), + subscription_options); } + this->_imu_subscription_ = this->create_subscription( + "/pacsim/imu/cog_imu", 1, + std::bind(&PacsimAdapter::_pacsim_imu_subscription_callback, this, std::placeholders::_1), + subscription_options); + this->_finished_client_ = this->create_client("/pacsim/finish_signal"); param_client_ = this->create_client("/pacsim/pacsim_node/get_parameters"); @@ -47,6 +57,7 @@ void PacsimAdapter::fetch_discipline() { if (!response->values.empty() && response->values[0].type == 4) { // Type 4 = string std::string discipline = response->values[0].string_value; + RCLCPP_INFO(this->get_logger(), "Discipline received: %s", discipline.c_str()); if (discipline == "skidpad") { mission_result = common_lib::competition_logic::Mission::SKIDPAD; @@ -67,12 +78,14 @@ void PacsimAdapter::fetch_discipline() { void PacsimAdapter::finish() { this->_finished_client_->async_send_request( std::make_shared(), - [this](rclcpp::Client::SharedFuture) {}); + [this](rclcpp::Client::SharedFuture) { + RCLCPP_INFO(this->get_logger(), "Finished signal sent"); + }); } void PacsimAdapter::_pacsim_perception_subscription_callback( const pacsim::msg::PerceptionDetections& msg) { - custom_interfaces::msg::PerceptionOutput cone_array_msg; + custom_interfaces::msg::ConeArray cone_array_msg; for (const pacsim::msg::PerceptionDetection& detection : msg.detections) { custom_interfaces::msg::Point2d position = custom_interfaces::msg::Point2d(); position.x = detection.pose.pose.position.x; @@ -82,11 +95,14 @@ void PacsimAdapter::_pacsim_perception_subscription_callback( cone_message.confidence = detection.detection_probability; cone_message.color = common_lib::competition_logic::get_color_string( common_lib::competition_logic::Color::UNKNOWN); - cone_array_msg.cones.cone_array.push_back(cone_message); + cone_array_msg.cone_array.push_back(cone_message); } cone_array_msg.header.stamp = msg.header.stamp; - cone_array_msg.exec_time = 0.01; - _perception_subscription_callback(cone_array_msg); + custom_interfaces::msg::PerceptionOutput perception_output; + perception_output.header = cone_array_msg.header; + perception_output.cones = cone_array_msg; + perception_output.exec_time = 0.0; + _perception_subscription_callback(perception_output); } void PacsimAdapter::_pacsim_velocities_subscription_callback( @@ -98,7 +114,12 @@ void PacsimAdapter::_pacsim_velocities_subscription_callback( velocities.angular_velocity = msg.twist.twist.angular.z; velocities.header.stamp = msg.header.stamp; for (unsigned int i = 0; i < 9; i++) { - velocities.covariance[i] = 0.0; + velocities.covariance[i] = 0.00001; // Pacsim does not provide covariance, assume small value + // or else shit breaks if used by solver } _velocities_subscription_callback(velocities); +} + +void PacsimAdapter::_pacsim_imu_subscription_callback(const sensor_msgs::msg::Imu& msg) { + _imu_subscription_callback(msg); } \ No newline at end of file diff --git a/src/slam/src/adapter_slam/vehicle.cpp b/src/slam/src/adapter_slam/vehicle.cpp index 153444e0e..02a70e9e2 100644 --- a/src/slam/src/adapter_slam/vehicle.cpp +++ b/src/slam/src/adapter_slam/vehicle.cpp @@ -1,15 +1,22 @@ #include "adapter_slam/vehicle.hpp" +#include +#include + +#include #include #include "common_lib/competition_logic/color.hpp" #include "ros_node/slam_node.hpp" +#include "slam_solver/solver_traits/odometry_integrator_trait.hpp" VehicleAdapter::VehicleAdapter(const SLAMParameters& params) : SLAMNode(params) { _operational_status_subscription_ = this->create_subscription( "/vehicle/operational_status", 10, [this](const custom_interfaces::msg::OperationalStatus::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Operational status received. Mission: %d - Go: %d", + msg->as_mission, msg->go_signal); _go_ = true; // msg->go_signal; _mission_ = common_lib::competition_logic::Mission(msg->as_mission); this->_slam_solver_->set_mission(_mission_); @@ -34,6 +41,23 @@ VehicleAdapter::VehicleAdapter(const SLAMParameters& params) : SLAMNode(params) transformStamped.transform.rotation.w = 1.0; _tf_static_broadcaster_->sendTransform(transformStamped); + + rclcpp::SubscriptionOptions subscription_options; + subscription_options.callback_group = this->_callback_group_; + + // LiDAR odometry subscription + if (!params.receive_lidar_odometry_) { + RCLCPP_INFO(this->get_logger(), "Not receiving lidar odometry, using only velocities"); + return; + } + RCLCPP_INFO(this->get_logger(), "VehicleAdapter initialized, topic for lidar odometry: %s", + params.lidar_odometry_topic_.c_str()); + + this->_lidar_odometry_subscription_ = this->create_subscription( + params.lidar_odometry_topic_, 1, + std::bind(&VehicleAdapter::_lidar_odometry_subscription_callback, this, + std::placeholders::_1), + subscription_options); } // TODO: implement a more complex logic, like the one in inspection node @@ -42,8 +66,41 @@ void VehicleAdapter::finish() { std::make_shared(), [this](rclcpp::Client::SharedFuture future) { if (future.get()->success) { + RCLCPP_INFO(this->get_logger(), "Finished signal sent"); } else { RCLCPP_ERROR(this->get_logger(), "Failed to send finished signal"); } }); +} + +void VehicleAdapter::_lidar_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { + if (this->_mission_ == common_lib::competition_logic::Mission::NONE) { + return; + } + + rclcpp::Time start_time = this->get_clock()->now(); + if (auto solver_ptr = std::dynamic_pointer_cast(this->_slam_solver_)) { + common_lib::structures::Pose pose; + pose.position.x = msg.pose.pose.position.x; + pose.position.y = msg.pose.pose.position.y; + tf2::Quaternion quat(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, msg.pose.pose.orientation.w); + + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + pose.orientation = yaw; // Use yaw as the orientation + pose.timestamp = msg.header.stamp; + + solver_ptr->add_odometry(pose); + this->_vehicle_pose_ = this->_slam_solver_->get_pose_estimate(); + } + + this->_publish_vehicle_pose(); + + rclcpp::Time end_time = this->get_clock()->now(); + this->_execution_times_->at(0) = (end_time - start_time).seconds() * 1000.0; + std_msgs::msg::Float64MultiArray execution_time_msg; + execution_time_msg.data = *this->_execution_times_; + this->_execution_time_publisher_->publish(execution_time_msg); } \ No newline at end of file diff --git a/src/slam/src/main.cpp b/src/slam/src/main.cpp index 23614e078..358888e71 100644 --- a/src/slam/src/main.cpp +++ b/src/slam/src/main.cpp @@ -19,8 +19,8 @@ int main(int argc, char **argv) { std::shared_ptr slam_node = adapter_map.at(adapter_type)(params); slam_node->init(); - // If graph slam and asynchronous optimization, use a multi-threaded executor - if (params.slam_solver_name_ != "ekf_slam" && params.slam_optimization_period_ != 0.0) { + // If graph slam, use a multi-threaded executor + if (params.slam_solver_name_ != "ekf_slam") { rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(slam_node); executor.spin(); diff --git a/src/slam/src/ros_node/slam_node.cpp b/src/slam/src/ros_node/slam_node.cpp index 11f21810d..d36b801fb 100644 --- a/src/slam/src/ros_node/slam_node.cpp +++ b/src/slam/src/ros_node/slam_node.cpp @@ -20,6 +20,7 @@ SLAMNode::SLAMNode(const SLAMParameters ¶ms) : Node("slam") { // Print parameters + RCLCPP_INFO_STREAM(this->get_logger(), "SLAM Node parameters:" << params); // Initialize the models std::shared_ptr motion_model = v2p_models_map.at(params.motion_model_name_)(); std::shared_ptr data_association = @@ -33,30 +34,44 @@ SLAMNode::SLAMNode(const SLAMParameters ¶ms) : Node("slam") { params.minimum_frequency_of_detections_), data_association); - this->_execution_times_ = std::make_shared>(20, 0.0); - std::shared_ptr loop_closure = std::make_shared(8, 15, 20, 3); + this->_execution_times_ = std::make_shared>(20, 0.0); // Preallocate 20 slots + std::shared_ptr loop_closure = std::make_shared(4, 10, 5, 3); // Initialize SLAM solver object this->_slam_solver_ = slam_solver_constructors_map.at(params.slam_solver_name_)( params, data_association, motion_model, landmark_filter, this->_execution_times_, loop_closure); + this->_vehicle_frame_id_ = params.frame_id_; _perception_map_ = std::vector(); _vehicle_state_velocities_ = common_lib::structures::Velocities(); _track_map_ = std::vector(); _vehicle_pose_ = common_lib::structures::Pose(); + // To control if callbacks are concurrent or not + if (params.slam_solver_name_ == "graph_slam" && params.slam_optimization_mode_ != "sync") { + this->_callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::Reentrant); // Allow callbacks to execute in parallel + } else { + this->_callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); // Default callback group + } + rclcpp::SubscriptionOptions subscription_options; + subscription_options.callback_group = this->_callback_group_; + // Subscriptions if (!params.use_simulated_perception_) { this->_perception_subscription_ = this->create_subscription( "/perception/cones", 1, - std::bind(&SLAMNode::_perception_subscription_callback, this, std::placeholders::_1)); + std::bind(&SLAMNode::_perception_subscription_callback, this, std::placeholders::_1), + subscription_options); } if (!params.use_simulated_velocities_) { this->_velocities_subscription_ = this->create_subscription( - "/state_estimation/velocities", 1, - std::bind(&SLAMNode::_velocities_subscription_callback, this, std::placeholders::_1)); + "/state_estimation/velocities", 50, + std::bind(&SLAMNode::_velocities_subscription_callback, this, std::placeholders::_1), + subscription_options); } // Publishers @@ -67,74 +82,68 @@ SLAMNode::SLAMNode(const SLAMParameters ¶ms) : Node("slam") { this->_visualization_map_publisher_ = this->create_publisher( "/state_estimation/visualization_map", 10); - this->_visualization_perception_map_publisher_ = - this->create_publisher( - "/state_estimation/visualization_map_perception", 10); this->_associations_visualization_publisher_ = this->create_publisher( "/state_estimation/visualization_associations", 10); - this->_position_publisher_ = this->create_publisher( - "/state_estimation/visualization/position", 10); + this->_trajectory_visualization_publisher_ = + this->create_publisher( + "/state_estimation/visualization/trajectory", 10); this->_execution_time_publisher_ = this->create_publisher( "/state_estimation/slam_execution_time", 10); this->_covariance_publisher_ = this->create_publisher( "/state_estimation/slam_covariance", 10); this->_lap_counter_publisher_ = this->create_publisher("/state_estimation/lap_counter", 10); - - this->_perception_delta_publisher_ = - this->create_publisher("/perception_delta", 10); - this->_tf_broadcaster_ = std::make_shared(this); + + RCLCPP_INFO(this->get_logger(), "SLAM Node has been initialized"); } -void SLAMNode::init() { this->_slam_solver_->init(this->weak_from_this()); } +void SLAMNode::init() { + if (auto solver_ptr = std::dynamic_pointer_cast(this->_slam_solver_)) { + solver_ptr->init(this->weak_from_this()); + } +} /*---------------------- Subscriptions --------------------*/ void SLAMNode::_perception_subscription_callback( const custom_interfaces::msg::PerceptionOutput &msg) { auto const &cone_array = msg.cones.cone_array; + auto const perception_exec_time = msg.exec_time; + + RCLCPP_DEBUG(this->get_logger(), "SUB - Perception: %ld cones. Mission: %d", cone_array.size(), + static_cast(this->_mission_)); if (!this->_go_ || this->_mission_ == common_lib::competition_logic::Mission::NONE) { + RCLCPP_INFO(this->get_logger(), "ATTR - Mission not started yet. Mission: %d - Go: %d ", + static_cast(this->_mission_), this->_go_); return; - } + } // Decide if start on mission or go etc... rclcpp::Time start_time = this->get_clock()->now(); - rclcpp::Time current_stamp(msg.header.stamp); - if (_last_perception_message_time_.nanoseconds() != 0) { - double delta_ms = (current_stamp - _last_perception_message_time_).seconds() * 1000.0; - - std_msgs::msg::Float64 msg; - msg.data = delta_ms; - _perception_delta_publisher_->publish(msg); + if (this->_slam_solver_ == nullptr) { + RCLCPP_WARN(this->get_logger(), "ATTR - Slam Solver object is null"); + return; } - // Update last timestamp - _last_perception_message_time_ = current_stamp; - this->_perception_map_.clear(); - Eigen::Vector3d velocities(this->_vehicle_state_velocities_.velocity_x, - this->_vehicle_state_velocities_.velocity_y, - this->_vehicle_state_velocities_.rotational_velocity); - double perception_exec_time = msg.exec_time; - double theta = -velocities(2) * perception_exec_time; + + // Compensate for motion during perception delay (See if now is needed) + double theta = -this->_vehicle_state_velocities_.rotational_velocity * perception_exec_time; double cos_theta = std::cos(theta); double sin_theta = std::sin(theta); for (auto &cone : cone_array) { - double x_linear_compensated = cone.position.x - velocities(0) * perception_exec_time; - double y_linear_compensated = cone.position.y - velocities(1) * perception_exec_time; + double x_linear_compensated = + cone.position.x - this->_vehicle_state_velocities_.velocity_x * perception_exec_time; + double y_linear_compensated = + cone.position.y - this->_vehicle_state_velocities_.velocity_y * perception_exec_time; double x_compensated = cos_theta * x_linear_compensated - sin_theta * y_linear_compensated; double y_compensated = sin_theta * x_linear_compensated + cos_theta * y_linear_compensated; this->_perception_map_.push_back(common_lib::structures::Cone( x_compensated, y_compensated, cone.color, cone.confidence, msg.header.stamp)); } - if (this->_slam_solver_ == nullptr) { - RCLCPP_WARN(this->get_logger(), "ATTR - Slam Solver object is null"); - return; - } - this->_slam_solver_->add_observations(this->_perception_map_); this->_track_map_ = this->_slam_solver_->get_map_estimate(); this->_vehicle_pose_ = this->_slam_solver_->get_pose_estimate(); @@ -156,7 +165,6 @@ void SLAMNode::_perception_subscription_callback( // Timekeeping rclcpp::Time end_time = this->get_clock()->now(); this->_execution_times_->at(1) = (end_time - start_time).seconds() * 1000.0; - std_msgs::msg::Float64MultiArray execution_time_msg; execution_time_msg.data = *this->_execution_times_; this->_execution_time_publisher_->publish(execution_time_msg); @@ -166,37 +174,44 @@ void SLAMNode::_velocities_subscription_callback(const custom_interfaces::msg::V if (this->_mission_ == common_lib::competition_logic::Mission::NONE) { return; } + RCLCPP_DEBUG(this->get_logger(), "SUB - Velocities: (%f, %f, %f), timestamp: %f", msg.velocity_x, + msg.velocity_y, msg.angular_velocity, + msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9); rclcpp::Time start_time = this->get_clock()->now(); - rclcpp::Time time1, time2, time3, time4, time5, time6; + if (auto solver_ptr = std::dynamic_pointer_cast(this->_slam_solver_)) { + this->_vehicle_state_velocities_ = common_lib::structures::Velocities( + msg.velocity_x, msg.velocity_y, msg.angular_velocity, msg.covariance[0], msg.covariance[4], + msg.covariance[8], msg.header.stamp); - this->_vehicle_state_velocities_ = common_lib::structures::Velocities( - msg.velocity_x, msg.velocity_y, msg.angular_velocity, msg.covariance[0], msg.covariance[4], - msg.covariance[8], msg.header.stamp); - time2 = this->get_clock()->now(); - - this->_slam_solver_->add_motion_prior(this->_vehicle_state_velocities_); - time3 = this->get_clock()->now(); + solver_ptr->add_velocities(this->_vehicle_state_velocities_); + } this->_vehicle_pose_ = this->_slam_solver_->get_pose_estimate(); - time4 = this->get_clock()->now(); - this->_track_map_ = this->_slam_solver_->get_map_estimate(); - time5 = this->get_clock()->now(); // this->_publish_covariance(); // TODO: get covariance to work fast this->_publish_vehicle_pose(); - time6 = this->get_clock()->now(); rclcpp::Time end_time = this->get_clock()->now(); this->_execution_times_->at(0) = (end_time - start_time).seconds() * 1000.0; - this->_execution_times_->at(11) = (time2 - start_time).seconds() * 1000.0; - this->_execution_times_->at(12) = (time3 - time2).seconds() * 1000.0; - this->_execution_times_->at(13) = (time4 - time3).seconds() * 1000.0; - this->_execution_times_->at(14) = (time5 - time4).seconds() * 1000.0; - this->_execution_times_->at(15) = (time6 - time5).seconds() * 1000.0; std_msgs::msg::Float64MultiArray execution_time_msg; execution_time_msg.data = *this->_execution_times_; this->_execution_time_publisher_->publish(execution_time_msg); } +void SLAMNode::_imu_subscription_callback(const sensor_msgs::msg::Imu &msg) { + if (this->_mission_ == common_lib::competition_logic::Mission::NONE) { + return; + } + RCLCPP_DEBUG(this->get_logger(), "SUB - IMU data received"); + if (auto solver_ptr = std::dynamic_pointer_cast(this->_slam_solver_)) { + auto imu_data = common_lib::sensor_data::ImuData( + msg.angular_velocity.z, msg.linear_acceleration.x, msg.linear_acceleration.y, + msg.header.stamp, msg.angular_velocity_covariance[8], msg.linear_acceleration_covariance[0], + msg.linear_acceleration_covariance[4]); + + solver_ptr->add_imu_data(imu_data); + } +} + /*---------------------- Publications --------------------*/ void SLAMNode::_publish_vehicle_pose() { @@ -205,15 +220,16 @@ void SLAMNode::_publish_vehicle_pose() { message.x = this->_vehicle_pose_.position.x; message.y = this->_vehicle_pose_.position.y; message.theta = this->_vehicle_pose_.orientation; - // TODO(marhcouto): add covariance + // TODO: add covariance message.header.stamp = this->get_clock()->now(); + RCLCPP_DEBUG(this->get_logger(), "PUB - Pose: (%f, %f, %f)", message.x, message.y, message.theta); this->_vehicle_pose_publisher_->publish(message); // Publish the transform tf_message.header.stamp = message.header.stamp; tf_message.header.frame_id = "map"; - tf_message.child_frame_id = "vehicle_estimate"; + tf_message.child_frame_id = this->_vehicle_frame_id_; tf2::Quaternion q; q.setRPY(0.0, 0.0, message.theta); @@ -225,17 +241,28 @@ void SLAMNode::_publish_vehicle_pose() { tf_message.transform.rotation.z = q.z(); tf_message.transform.rotation.w = q.w(); this->_tf_broadcaster_->sendTransform(tf_message); + + // Publish trajectory marker + if (auto solver_ptr = std::dynamic_pointer_cast(this->_slam_solver_)) { + std::vector trajectory = solver_ptr->get_trajectory_estimate(); + auto marker_array_msg = visualization_msgs::msg::MarkerArray(); + marker_array_msg = common_lib::communication::marker_array_from_structure_array( + trajectory, "vehicle_trajectory", "map", "blue", "sphere"); + this->_trajectory_visualization_publisher_->publish(marker_array_msg); + } } void SLAMNode::_publish_map() { auto cone_array_msg = custom_interfaces::msg::ConeArray(); auto marker_array_msg = visualization_msgs::msg::MarkerArray(); auto marker_array_msg_perception = visualization_msgs::msg::MarkerArray(); + // RCLCPP_DEBUG(this->get_logger(), "PUB - cone map %ld", this->_track_map_.size()); + // RCLCPP_DEBUG(this->get_logger(), "--------------------------------------"); for (common_lib::structures::Cone const &cone : this->_track_map_) { auto cone_message = custom_interfaces::msg::Cone(); cone_message.position.x = cone.position.x; cone_message.position.y = cone.position.y; - // TODO(marhcouto): add covariance & large cones & confidence + // TODO: add covariance & large cones & confidence cone_message.color = common_lib::competition_logic::get_color_string(cone.color); cone_array_msg.cone_array.push_back(cone_message); // RCLCPP_DEBUG(this->get_logger(), "(%f\t%f)\t%s", cone_message.position.x, @@ -279,6 +306,7 @@ void SLAMNode::_publish_lap_counter() { void SLAMNode::_publish_associations() { auto marker_array_msg = visualization_msgs::msg::MarkerArray(); + RCLCPP_DEBUG(this->get_logger(), "PUB - Associations %ld", this->_associations_.size()); // Add a DELETE_ALL marker to clear previous markers visualization_msgs::msg::Marker clear_marker; clear_marker.header.frame_id = "map"; diff --git a/src/slam/src/slam_config/general_config.cpp b/src/slam/src/slam_config/general_config.cpp index 33696221a..1adb46cf3 100644 --- a/src/slam/src/slam_config/general_config.cpp +++ b/src/slam/src/slam_config/general_config.cpp @@ -8,12 +8,17 @@ SLAMParameters::SLAMParameters(const SLAMParameters ¶ms) { use_simulated_perception_ = params.use_simulated_perception_; use_simulated_velocities_ = params.use_simulated_velocities_; motion_model_name_ = params.motion_model_name_; + pose_updater_name_ = params.pose_updater_name_; + landmark_filter_name_ = params.landmark_filter_name_; + lidar_odometry_topic_ = params.lidar_odometry_topic_; + receive_lidar_odometry_ = params.receive_lidar_odometry_; data_association_model_name_ = params.data_association_model_name_; data_association_limit_distance_ = params.data_association_limit_distance_; observation_x_noise_ = params.observation_x_noise_; observation_y_noise_ = params.observation_y_noise_; velocity_x_noise_ = params.velocity_x_noise_; velocity_y_noise_ = params.velocity_y_noise_; + imu_acceleration_x_noise_ = params.imu_acceleration_x_noise_; angular_velocity_noise_ = params.angular_velocity_noise_; pose_x_initial_noise_ = params.pose_x_initial_noise_; pose_y_initial_noise_ = params.pose_y_initial_noise_; @@ -22,17 +27,23 @@ SLAMParameters::SLAMParameters(const SLAMParameters ¶ms) { slam_solver_name_ = params.slam_solver_name_; slam_min_pose_difference_ = params.slam_min_pose_difference_; slam_optimization_period_ = params.slam_optimization_period_; - landmark_filter_name_ = params.landmark_filter_name_; - minimum_frequency_of_detections_ = params.minimum_frequency_of_detections_; - minimum_observation_count_ = params.minimum_observation_count_; frame_id_ = params.frame_id_; slam_optimization_type_ = params.slam_optimization_type_; + slam_optimization_in_poses_ = params.slam_optimization_in_poses_; slam_optimization_mode_ = params.slam_optimization_mode_; preloaded_map_noise_ = params.preloaded_map_noise_; slam_isam2_relinearize_threshold_ = params.slam_isam2_relinearize_threshold_; slam_isam2_relinearize_skip_ = params.slam_isam2_relinearize_skip_; slam_isam2_factorization_ = params.slam_isam2_factorization_; sliding_window_size_ = params.sliding_window_size_; + + minimum_observation_count_ = params.minimum_observation_count_; + minimum_frequency_of_detections_ = params.minimum_frequency_of_detections_; + + threshold_dist = params.threshold_dist; + first_x_cones = params.first_x_cones; + border_width = params.border_width; + minimum_confidence = params.minimum_confidence; } SLAMParameters &SLAMParameters::operator=(const SLAMParameters &other) { @@ -40,12 +51,17 @@ SLAMParameters &SLAMParameters::operator=(const SLAMParameters &other) { use_simulated_perception_ = other.use_simulated_perception_; use_simulated_velocities_ = other.use_simulated_velocities_; motion_model_name_ = other.motion_model_name_; + pose_updater_name_ = other.pose_updater_name_; + landmark_filter_name_ = other.landmark_filter_name_; + lidar_odometry_topic_ = other.lidar_odometry_topic_; + receive_lidar_odometry_ = other.receive_lidar_odometry_; data_association_model_name_ = other.data_association_model_name_; data_association_limit_distance_ = other.data_association_limit_distance_; observation_x_noise_ = other.observation_x_noise_; observation_y_noise_ = other.observation_y_noise_; velocity_x_noise_ = other.velocity_x_noise_; velocity_y_noise_ = other.velocity_y_noise_; + imu_acceleration_x_noise_ = other.imu_acceleration_x_noise_; angular_velocity_noise_ = other.angular_velocity_noise_; pose_x_initial_noise_ = other.pose_x_initial_noise_; pose_y_initial_noise_ = other.pose_y_initial_noise_; @@ -56,12 +72,21 @@ SLAMParameters &SLAMParameters::operator=(const SLAMParameters &other) { slam_optimization_period_ = other.slam_optimization_period_; frame_id_ = other.frame_id_; slam_optimization_type_ = other.slam_optimization_type_; + slam_optimization_in_poses_ = other.slam_optimization_in_poses_; slam_optimization_mode_ = other.slam_optimization_mode_; preloaded_map_noise_ = other.preloaded_map_noise_; slam_isam2_relinearize_threshold_ = other.slam_isam2_relinearize_threshold_; slam_isam2_relinearize_skip_ = other.slam_isam2_relinearize_skip_; slam_isam2_factorization_ = other.slam_isam2_factorization_; sliding_window_size_ = other.sliding_window_size_; + + minimum_observation_count_ = other.minimum_observation_count_; + minimum_frequency_of_detections_ = other.minimum_frequency_of_detections_; + + threshold_dist = other.threshold_dist; + first_x_cones = other.first_x_cones; + border_width = other.border_width; + minimum_confidence = other.minimum_confidence; } return *this; } @@ -83,6 +108,10 @@ std::string SLAMParameters::load_config() { YAML::Node slam_config = YAML::LoadFile(slam_config_path); this->motion_model_name_ = slam_config["slam"]["motion_model_name"].as(); + this->pose_updater_name_ = slam_config["slam"]["pose_updater_name"].as(); + this->landmark_filter_name_ = slam_config["slam"]["landmark_filter_name"].as(); + this->lidar_odometry_topic_ = slam_config["slam"]["lidar_odometry_topic"].as(); + this->receive_lidar_odometry_ = slam_config["slam"]["receive_lidar_odometry"].as(); this->data_association_model_name_ = slam_config["slam"]["data_association_model_name"].as(); this->data_association_limit_distance_ = @@ -94,6 +123,7 @@ std::string SLAMParameters::load_config() { this->observation_y_noise_ = slam_config["slam"]["observation_y_noise"].as(); this->velocity_x_noise_ = slam_config["slam"]["velocity_x_noise"].as(); this->velocity_y_noise_ = slam_config["slam"]["velocity_y_noise"].as(); + this->imu_acceleration_x_noise_ = slam_config["slam"]["imu_acceleration_x_noise"].as(); this->angular_velocity_noise_ = slam_config["slam"]["angular_velocity_noise"].as(); this->pose_x_initial_noise_ = slam_config["slam"]["pose_x_initial_noise"].as(); this->pose_y_initial_noise_ = slam_config["slam"]["pose_y_initial_noise"].as(); @@ -102,11 +132,8 @@ std::string SLAMParameters::load_config() { this->slam_solver_name_ = slam_config["slam"]["slam_solver_name"].as(); this->slam_min_pose_difference_ = slam_config["slam"]["slam_min_pose_difference"].as(); this->slam_optimization_period_ = slam_config["slam"]["slam_optimization_period"].as(); - this->landmark_filter_name_ = slam_config["slam"]["landmark_filter_name"].as(); - this->minimum_observation_count_ = slam_config["slam"]["minimum_observation_count"].as(); - this->minimum_frequency_of_detections_ = - slam_config["slam"]["minimum_frequency_of_detections"].as(); this->slam_optimization_type_ = slam_config["slam"]["slam_optimization_type"].as(); + this->slam_optimization_in_poses_ = slam_config["slam"]["slam_optimization_in_poses"].as(); this->slam_optimization_mode_ = slam_config["slam"]["slam_optimization_mode"].as(); this->preloaded_map_noise_ = slam_config["slam"]["preloaded_map_noise"].as(); this->slam_isam2_relinearize_threshold_ = @@ -116,5 +143,14 @@ std::string SLAMParameters::load_config() { this->slam_isam2_factorization_ = slam_config["slam"]["slam_isam2_factorization"].as(); this->sliding_window_size_ = slam_config["slam"]["sliding_window_size"].as(); + + this->minimum_observation_count_ = slam_config["slam"]["minimum_observation_count"].as(); + this->minimum_frequency_of_detections_ = + slam_config["slam"]["minimum_frequency_of_detections"].as(); + + this->threshold_dist = slam_config["slam"]["threshold_dist"].as(); + this->first_x_cones = slam_config["slam"]["first_x_cones"].as(); + this->border_width = slam_config["slam"]["border_width"].as(); + this->minimum_confidence = slam_config["slam"]["minimum_confidence"].as(); return adapter; } \ No newline at end of file diff --git a/src/slam/src/slam_solver/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp index 25d61209b..722863b7d 100644 --- a/src/slam/src/slam_solver/ekf_slam_solver.cpp +++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp @@ -20,8 +20,6 @@ EKFSLAMSolver::EKFSLAMSolver(const SLAMParameters& params, this->observation_model_ = std::make_shared(); } -void EKFSLAMSolver::init([[maybe_unused]] std::weak_ptr _) {} - Eigen::MatrixXd EKFSLAMSolver::get_observation_noise_matrix(int num_landmarks) const { Eigen::MatrixXd observation_noise_matrix = Eigen::MatrixXd::Zero(num_landmarks * 2, num_landmarks * 2); @@ -32,7 +30,7 @@ Eigen::MatrixXd EKFSLAMSolver::get_observation_noise_matrix(int num_landmarks) c return observation_noise_matrix; } -void EKFSLAMSolver::add_motion_prior(const common_lib::structures::Velocities& velocities) { +void EKFSLAMSolver::add_velocities(const common_lib::structures::Velocities& velocities) { if (velocities_received_) { predict(this->state_, this->covariance_, process_noise_matrix_, this->last_update_, velocities); } else { @@ -42,6 +40,7 @@ void EKFSLAMSolver::add_motion_prior(const common_lib::structures::Velocities& v } void EKFSLAMSolver::add_observations(const std::vector& cones) { + // Prepare data structures int num_observations = static_cast(cones.size()); std::vector matched_landmarks_indices; Eigen::VectorXd matched_observations(0); @@ -52,10 +51,16 @@ void EKFSLAMSolver::add_observations(const std::vectorstate_.segment(0, 3), observations); + + // Data association Eigen::VectorXi associations = this->_data_association_->associate( this->state_.segment(3, this->state_.size() - 3), global_observations, this->covariance_.block(3, 3, this->state_.size() - 3, this->state_.size() - 3), observation_confidences); + + // Perception Filter + Eigen::VectorXd filtered_landmarks = + this->_landmark_filter_->filter(global_observations, observation_confidences, associations); for (int i = 0; i < num_observations; ++i) { if (associations(i) == -2) { continue; @@ -72,18 +77,18 @@ void EKFSLAMSolver::add_observations(const std::vector_landmark_filter_->filter(new_landmarks, new_confidences); - this->_landmark_filter_->delete_landmarks(filtered_new_landmarks); + + // Update Kalman Filter this->correct(this->state_, this->covariance_, matched_landmarks_indices, matched_observations); if (this->_mission_ != common_lib::competition_logic::Mission::NONE && !(this->_params_.using_preloaded_map_ && (this->_mission_ == common_lib::competition_logic::Mission::SKIDPAD || this->_mission_ == common_lib::competition_logic::Mission::ACCELERATION)) && this->lap_counter_ == 0) { - this->state_augmentation(this->state_, this->covariance_, filtered_new_landmarks); + this->state_augmentation(this->state_, this->covariance_, new_landmarks); this->update_process_noise_matrix(); } + this->_landmark_filter_->delete_landmarks(new_landmarks); } void EKFSLAMSolver::predict(Eigen::VectorXd& state, Eigen::MatrixXd& covariance, @@ -147,7 +152,7 @@ void EKFSLAMSolver::state_augmentation(Eigen::VectorXd& state, Eigen::MatrixXd& state.segment(original_state_size, num_new_entries) = new_landmarks_coordinates; } -void EKFSLAMSolver::load_initial_state(const Eigen::VectorXd& map, const Eigen::VectorXd& pose) { +void EKFSLAMSolver::load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) { if (map.size() % 2 != 0 || pose.size() != 3) { throw std::runtime_error("Invalid map or pose size"); } @@ -165,10 +170,14 @@ void EKFSLAMSolver::load_initial_state(const Eigen::VectorXd& map, const Eigen:: std::vector EKFSLAMSolver::get_map_estimate() { std::vector map; + std::string mapa = ""; for (int i = 3; i < this->state_.size(); i += 2) { + mapa += + "(" + std::to_string(this->state_(i)) + ", " + std::to_string(this->state_(i + 1)) + "), "; map.push_back(common_lib::structures::Cone(this->state_(i), this->state_(i + 1), "unknown", 1.0, this->last_update_)); } + RCLCPP_INFO(rclcpp::get_logger("slam_solver"), "Map estimate: %s", mapa.c_str()); return map; } diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp index 9a8fbb1cf..5a701431c 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp @@ -16,6 +16,7 @@ #include #include "common_lib/maths/transformations.hpp" +#include "slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp" #include "slam_solver/graph_slam_solver/utils.hpp" bool GraphSLAMInstance::new_pose_factors() const { return this->_new_pose_node_; } @@ -79,11 +80,17 @@ GraphSLAMInstance::GraphSLAMInstance(const SLAMParameters& params, : _params_(params), _optimizer_(optimizer) { // Create a new factor graph _factor_graph_ = gtsam::NonlinearFactorGraph(); - const gtsam::Pose2 prior_pose(0.0, 0.0, 0.0); + const gtsam::Pose2 prior_pose(0.0, 0.0, + 0.0); // Create initial prior pose at origin, to bind graph const gtsam::Symbol pose_symbol('x', ++(this->_pose_counter_)); const gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.0, 0.0, 0.0)); _factor_graph_.add(gtsam::PriorFactor(pose_symbol, prior_pose, prior_noise)); + if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { + optimizer_ptr->_new_factors_.add( + gtsam::PriorFactor(pose_symbol, prior_pose, prior_noise)); + optimizer_ptr->_new_values_.insert(pose_symbol, prior_pose); + } // Create a new values object _graph_values_ = gtsam::Values(); @@ -99,9 +106,8 @@ GraphSLAMInstance::GraphSLAMInstance(const GraphSLAMInstance& other) { _landmark_counter_ = other._landmark_counter_; _new_pose_node_ = other._new_pose_node_; _new_observation_factors_ = other._new_observation_factors_; - _optimizer_ = other._optimizer_; + _optimizer_ = other._optimizer_->clone(); // because it's a shared_ptr _params_ = other._params_; - _accumulated_pose_difference_ = other._accumulated_pose_difference_; } GraphSLAMInstance& GraphSLAMInstance::operator=(const GraphSLAMInstance& other) { @@ -114,60 +120,68 @@ GraphSLAMInstance& GraphSLAMInstance::operator=(const GraphSLAMInstance& other) _landmark_counter_ = other._landmark_counter_; _new_pose_node_ = other._new_pose_node_; _new_observation_factors_ = other._new_observation_factors_; - _optimizer_ = other._optimizer_; + _optimizer_ = other._optimizer_->clone(); // because it's a shared_ptr _params_ = other._params_; - _accumulated_pose_difference_ = other._accumulated_pose_difference_; return *this; } -bool GraphSLAMInstance::process_pose_difference(const Eigen::Vector3d& pose_difference, - const Eigen::Vector3d& new_pose, - bool force_update) { - this->_accumulated_pose_difference_ += pose_difference; - if (double pose_difference_norm = ::sqrt(pow(_accumulated_pose_difference_(0), 2) + - pow(_accumulated_pose_difference_(1), 2) + - pow(_accumulated_pose_difference_(2), 2)); - pose_difference_norm < this->_params_.slam_min_pose_difference_ && !force_update) { - return false; - } - +void GraphSLAMInstance::process_new_pose(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector, + const Eigen::Vector3d& new_pose) { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "GraphSLAMInstance - Processing new pose %lf %lf %lf", + new_pose(0), new_pose(1), new_pose(2)); gtsam::Pose2 new_pose_gtsam = eigen_to_gtsam_pose(new_pose); - gtsam::Pose2 pose_difference_gtsam = this->get_pose().between(new_pose_gtsam); - - // Calculate noise - // // TODO: Implement noise -> this was shit because covariance from velocity estimation had too - // // small error - // Eigen::Matrix3d velocities_noise = Eigen::Matrix3d::Identity(); velocities_noise(0, - // 0) = velocities.velocity_x_noise_; velocities_noise(1, 1) = velocities.velocity_y_noise_; - // velocities_noise(2, 2) = velocities.rotational_velocity_noise_; - // RCLCPP_DEBUG(rclcpp::get_logger("slam"), "Velocities noise: %f %f %f", velocities_noise(0, 0), - // velocities_noise(1, 1), velocities_noise(2, 2)); - // Eigen::Matrix3d velocities_jacobian = - // this->_motion_model_->get_jacobian_velocities(previous_pose, velocities_vector, delta_t); - // Eigen::Matrix3d motion_covariance = - // velocities_jacobian * velocities_noise * velocities_jacobian.transpose(); - // RCLCPP_DEBUG(rclcpp::get_logger("slam"), "Motion covariance: %f %f %f", motion_covariance(0, - // 0), - // motion_covariance(1, 1), motion_covariance(2, 2)); - gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Sigmas( - gtsam::Vector3(this->_params_.velocity_x_noise_, this->_params_.velocity_y_noise_, - this->_params_.angular_velocity_noise_)); - - // Add the prior factor to the graph (x means pose node) - gtsam::Symbol previous_pose_symbol('x', this->_pose_counter_); - gtsam::Symbol new_pose_symbol('x', ++(this->_pose_counter_)); - this->_factor_graph_.add(gtsam::BetweenFactor(previous_pose_symbol, new_pose_symbol, - pose_difference_gtsam, prior_noise)); + // X means pose node, l means landmark node + gtsam::Symbol new_pose_symbol('x', ++(this->_pose_counter_)); // Add the prior pose to the values _graph_values_.insert(new_pose_symbol, new_pose_gtsam); - this->_new_pose_node_ = true; - this->_accumulated_pose_difference_ = Eigen::Vector3d::Zero(); // Reset the accumulated pose - return true; + if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { + optimizer_ptr->_new_values_.insert(new_pose_symbol, new_pose_gtsam); + } + + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "GraphSLAMInstance - Adding prior pose %lf %lf %lf with noise %lf %lf %lf and " + "pose difference %lf %lf %lf", + new_pose_gtsam.x(), new_pose_gtsam.y(), new_pose_gtsam.theta(), noise_vector(0), + noise_vector(1), noise_vector(2), pose_difference(0), pose_difference(1), + pose_difference(2)); + + this->process_pose_difference(pose_difference, noise_vector); +} + +void GraphSLAMInstance::process_pose_difference(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector, + unsigned int before_pose_id, + unsigned int after_pose_id) { + gtsam::Symbol before_pose_symbol('x', before_pose_id); + gtsam::Symbol after_pose_symbol('x', after_pose_id); + gtsam::noiseModel::Diagonal::shared_ptr motion_noise = gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector3(noise_vector(0), noise_vector(1), noise_vector(2))); + gtsam::Pose2 pose_difference_gtsam = eigen_to_gtsam_pose(pose_difference); + this->_factor_graph_.add(gtsam::BetweenFactor(before_pose_symbol, after_pose_symbol, + pose_difference_gtsam, motion_noise)); + if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { + optimizer_ptr->_new_factors_.add(gtsam::BetweenFactor( + before_pose_symbol, after_pose_symbol, pose_difference_gtsam, motion_noise)); + } +} + +void GraphSLAMInstance::process_pose_difference(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector, + unsigned int before_pose_id) { + this->process_pose_difference(pose_difference, noise_vector, before_pose_id, + this->_pose_counter_); +} + +void GraphSLAMInstance::process_pose_difference(const Eigen::Vector3d& pose_difference, + const Eigen::Vector3d& noise_vector) { + this->process_pose_difference(pose_difference, noise_vector, this->_pose_counter_ - 1, + this->_pose_counter_); } void GraphSLAMInstance::process_observations(const ObservationData& observation_data) { @@ -188,7 +202,14 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ landmark_symbol = gtsam::Symbol('l', ++(this->_landmark_counter_)); landmark = gtsam::Point2(observations_global(i * 2), observations_global(i * 2 + 1)); this->_graph_values_.insert(landmark_symbol, landmark); + if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { + optimizer_ptr->_new_values_.insert( + landmark_symbol, landmark); // Most efficient way I found to deal with ISAM2 + } } else { + if (!this->new_pose_factors()) { // Only add old observations if the vehicle has moved + continue; + } // Association to previous landmark landmark_symbol = gtsam::Symbol('l', (associations(i)) / 2 + 1); // Convert to landmark id } @@ -199,9 +220,15 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ const gtsam::Rot2 observation_rotation(observation_cylindrical(1)); const gtsam::noiseModel::Diagonal::shared_ptr observation_noise = - gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(this->_params_.observation_x_noise_, - this->_params_.observation_y_noise_)); - + gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(this->_params_.observation_y_noise_, + this->_params_.observation_x_noise_)); + + if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { + optimizer_ptr->_new_factors_.add(gtsam::BearingRangeFactor( + gtsam::Symbol('x', this->_pose_counter_), landmark_symbol, observation_rotation, + observation_cylindrical(0), observation_noise)); // Again, just for ISAM2, because + // it's incremental + } this->_factor_graph_.add(gtsam::BearingRangeFactor( gtsam::Symbol('x', this->_pose_counter_), landmark_symbol, observation_rotation, observation_cylindrical(0), observation_noise)); @@ -210,8 +237,9 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ this->_new_observation_factors_ = new_observation_factors; } -void GraphSLAMInstance::load_initial_state(const Eigen::VectorXd& map, const Eigen::VectorXd& pose, +void GraphSLAMInstance::load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose, double preloaded_map_noise) { + RCLCPP_INFO(rclcpp::get_logger("slam"), "GraphSLAMInstance - Loading map "); this->_pose_counter_ = 0; this->_landmark_counter_ = 0; _factor_graph_ = gtsam::NonlinearFactorGraph(); @@ -238,7 +266,10 @@ void GraphSLAMInstance::load_initial_state(const Eigen::VectorXd& map, const Eig } } -void GraphSLAMInstance::optimize() { // TODO: implement sliding window and other parameters +void GraphSLAMInstance::optimize() { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "GraphSLAMInstance - Optimizing graph with %ld factors and %ld values", + this->_factor_graph_.size(), this->_graph_values_.size()); if (!this->_new_observation_factors_) return; this->_graph_values_ = this->_optimizer_->optimize( @@ -247,6 +278,7 @@ void GraphSLAMInstance::optimize() { // TODO: implement sliding window and othe } void GraphSLAMInstance::lock_landmarks(double locked_landmark_noise) { + // TODO: check if this works for (unsigned int i = 1; i < this->_landmark_counter_; i++) { gtsam::Symbol landmark_symbol('l', i); if (this->_graph_values_.exists(landmark_symbol)) { diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp index 4cf1fc474..4d6d42f0c 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp @@ -20,6 +20,8 @@ #include "common_lib/maths/transformations.hpp" #include "slam_solver/graph_slam_solver/optimizer/map.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/map.hpp" +#include "slam_solver/graph_slam_solver/pose_updater/pose_updater_traits/second_pose_input_trait.hpp" #include "slam_solver/graph_slam_solver/utils.hpp" GraphSLAMSolver::GraphSLAMSolver(const SLAMParameters& params, @@ -29,46 +31,125 @@ GraphSLAMSolver::GraphSLAMSolver(const SLAMParameters& params, std::shared_ptr> execution_times, std::shared_ptr loop_closure) : SLAMSolver(params, data_association, motion_model, landmark_filter, execution_times, - loop_closure), - _graph_slam_instance_(params, graph_slam_optimizer_constructors_map.at( - params.slam_optimization_type_)(params)) { - // TODO: transform into range and bearing noises + loop_closure) { + this->_graph_slam_instance_ = std::make_shared( + params, graph_slam_optimizer_constructors_map.at(params.slam_optimization_type_)(params)); + this->_pose_updater_ = pose_updater_constructors_map.at(params.pose_updater_name_)(params); + this->_odometry_model = std::make_shared(); } void GraphSLAMSolver::init([[maybe_unused]] std::weak_ptr node) { + if (const auto node_ptr = node.lock()) { + this->_reentrant_group_ = node_ptr->create_callback_group( + rclcpp::CallbackGroupType::Reentrant); // Allow callbacks to execute in parallel + } // Create a timer for asynchronous optimization if (this->_params_.slam_optimization_mode_ != "async" || this->_params_.slam_optimization_period_ <= 0.0) { + RCLCPP_INFO(rclcpp::get_logger("slam"), "Optimization is synchronous"); } else if (const auto node_ptr = node.lock()) { - this->_reentrant_group_ = node_ptr->create_callback_group( - rclcpp::CallbackGroupType::Reentrant); // Allow callbacks to execute in parallel this->_optimization_timer_ = node_ptr->create_wall_timer( std::chrono::milliseconds( static_cast(this->_params_.slam_optimization_period_ * 1000)), [this]() { this->_asynchronous_optimization_routine(); }, this->_reentrant_group_); + RCLCPP_INFO(rclcpp::get_logger("slam"), "Optimization timer created with period %f seconds", + this->_params_.slam_optimization_period_); } else { this->_params_.slam_optimization_period_ = 0.0; // Trigger optimization on observations + RCLCPP_ERROR(rclcpp::get_logger("slam"), + "Failed to create optimization timer, node is not valid"); + } +} + +bool GraphSLAMSolver::_add_motion_data_to_graph( + const std::shared_ptr pose_updater, + const std::shared_ptr graph_slam_instance, bool force_update) { + if (pose_updater->pose_ready_for_graph_update() || force_update) { + // If the pose updater is ready to update the pose, we can proceed with the pose update + // Eigen::Vector3d pose_difference_noise = {0.2, 0.2, 0.3}; + graph_slam_instance->process_new_pose( + pose_difference_eigen(pose_updater->get_last_graphed_pose(), pose_updater->get_last_pose()), + pose_updater->get_pose_difference_noise(), pose_updater->get_last_pose()); + + pose_updater->update_pose( + pose_updater->get_last_pose()); // Reset the accumulated pose difference + return true; } + + return false; } -void GraphSLAMSolver::add_motion_prior(const common_lib::structures::Velocities& velocities) { +void GraphSLAMSolver::add_odometry(const common_lib::structures::Pose& odometry) { std::unique_lock lock(this->_mutex_); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_odometry - Mutex accessed"); rclcpp::Time start_time, motion_model_time, factor_graph_time; + + // Apply motion model + MotionData odometry_data(std::make_shared(3), + std::make_shared(3), odometry.timestamp); + odometry_data.motion_data_->head<3>() << odometry.position.x, odometry.position.y, + odometry.orientation; + odometry_data.motion_data_noise_->head<3>() << odometry.position.x_noise, + odometry.position.y_noise, odometry.orientation_noise; start_time = rclcpp::Clock().now(); - MotionData velocities_data( - std::make_shared(velocities.velocity_x, velocities.velocity_y, - velocities.rotational_velocity), - velocities.timestamp_); - auto [pose_difference, new_pose] = - this->_pose_updater_.update_pose(velocities_data, this->_motion_model_); if (this->_optimization_under_way_) { - this->_motion_data_queue_.push(velocities_data); + this->_motion_data_queue_.push(odometry_data); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "add_odometry - Optimization under way, pushing motion data"); } + this->_pose_updater_->predict_pose(odometry_data, this->_odometry_model); motion_model_time = rclcpp::Clock().now(); - bool new_factor = this->_graph_slam_instance_.process_pose_difference(pose_difference, new_pose); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_odometry - Pose updated"); + + // Add motion data to the graph + bool added_factors = + this->_add_motion_data_to_graph(this->_pose_updater_, this->_graph_slam_instance_); factor_graph_time = rclcpp::Clock().now(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_odometry - Factors added? %d", added_factors); + + // Timekeeping + if (this->_execution_times_ == nullptr) { + return; + } + this->_execution_times_->at(9) = (motion_model_time - start_time).seconds() * 1000.0; + this->_execution_times_->at(10) = (factor_graph_time - motion_model_time).seconds() * 1000.0; + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_odometry - Mutex unlocked"); +} + +void GraphSLAMSolver::add_velocities(const common_lib::structures::Velocities& velocities) { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_velocities - Mutex waiting to lock"); + std::unique_lock lock(this->_mutex_); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_velocities - Mutex accessed"); + rclcpp::Time start_time, motion_model_time, factor_graph_time; + + // Construct motion data + MotionData velocities_data(std::make_shared(4), + std::make_shared(4), velocities.timestamp_); + velocities_data.motion_data_->head<4>() << velocities.velocity_x, velocities.velocity_y, + velocities.rotational_velocity, this->_last_imu_data_.acceleration_x; + velocities_data.motion_data_noise_->head<4>() << _params_.velocity_x_noise_, + _params_.velocity_y_noise_, _params_.angular_velocity_noise_, + _params_.imu_acceleration_x_noise_; + velocities_data.timestamp_ = velocities.timestamp_; + start_time = rclcpp::Clock().now(); + if (this->_optimization_under_way_) { + this->_motion_data_queue_.push(velocities_data); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "add_velocities - Optimization under way, pushing motion data"); + } + + // Apply motion model + this->_pose_updater_->predict_pose(velocities_data, this->_motion_model_); + motion_model_time = rclcpp::Clock().now(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_velocities - Pose updated: (%f, %f, %f)", + this->_pose_updater_->get_last_pose()(0), this->_pose_updater_->get_last_pose()(1), + this->_pose_updater_->get_last_pose()(2)); - this->_is_stopped_at_beginning_ &= !new_factor; + // Add motion data to the graph + bool added_factors = + this->_add_motion_data_to_graph(this->_pose_updater_, this->_graph_slam_instance_); + factor_graph_time = rclcpp::Clock().now(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_velocities - Factors added? %d", added_factors); // Timekeeping if (this->_execution_times_ == nullptr) { @@ -76,22 +157,31 @@ void GraphSLAMSolver::add_motion_prior(const common_lib::structures::Velocities& } this->_execution_times_->at(9) = (motion_model_time - start_time).seconds() * 1000.0; this->_execution_times_->at(10) = (factor_graph_time - motion_model_time).seconds() * 1000.0; + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_velocities - Mutex unlocked"); +} + +void GraphSLAMSolver::add_imu_data(const common_lib::sensor_data::ImuData& imu_data) { + this->_last_imu_data_ = imu_data; } void GraphSLAMSolver::add_observations(const std::vector& cones) { if (cones.empty()) { - this->_landmark_filter_->filter(Eigen::VectorXd::Zero(0), Eigen::VectorXd::Zero(0)); + // this->_landmark_filter_->filter(Eigen::VectorXd::Zero(0), Eigen::VectorXd::Zero(0), + // Eigen::VectorXi::Zero(0)); this->_observations_global_ = Eigen::VectorXd::Zero(0); this->_associations_ = Eigen::VectorXi::Zero(0); return; } + + // Prepare the data structures for observations rclcpp::Time start_time, initialization_time, covariance_time, association_time, factor_graph_time, optimization_time; start_time = rclcpp::Clock().now(); Eigen::VectorXd observations(cones.size() * 2); Eigen::VectorXd observations_confidences(cones.size()); Eigen::VectorXd observations_global(cones.size() * 2); - Eigen::VectorXd state; + Eigen::VectorXd landmarks, state; + Eigen::Vector3d pose; Eigen::MatrixXd covariance; for (unsigned int i = 0; i < cones.size(); i++) { // Iterate through the cones @@ -101,94 +191,50 @@ void GraphSLAMSolver::add_observations(const std::vector_mutex_); - // Reduce error associated to accumulated pose difference by creating new factor - if (this->_graph_slam_instance_.new_pose_factors() && !_is_stopped_at_beginning_) { - this->_graph_slam_instance_.process_pose_difference( - Eigen::Vector3d::Zero(), this->_pose_updater_.get_last_pose(), true); - } - state = this->_graph_slam_instance_.get_state_vector(); - observations_global = - common_lib::maths::local_to_global_coordinates(state.head<3>(), observations); + state = this->_graph_slam_instance_->get_state_vector(); + landmarks = state.segment(3, state.size() - 3); + pose = this->_pose_updater_->get_last_pose(); + observations_global = common_lib::maths::local_to_global_coordinates(pose, observations); initialization_time = rclcpp::Clock().now(); - covariance = this->_graph_slam_instance_.get_covariance_matrix(); + covariance = this->_graph_slam_instance_->get_covariance_matrix(); covariance_time = rclcpp::Clock().now(); } // Data association - Eigen::VectorXi associations(cones.size()); - associations = this->_data_association_->associate( - state.segment(3, state.size() - 3), observations_global, covariance, + Eigen::VectorXi associations = this->_data_association_->associate( + landmarks, observations_global, covariance, observations_confidences); // TODO: implement different mahalanobis distance this->_associations_ = associations; this->_observations_global_ = observations_global; this->_map_coordinates_ = state.segment(3, state.size() - 3); association_time = rclcpp::Clock().now(); - Eigen::VectorXd unfiltered_new_observations; - Eigen::VectorXd unfiltered_new_observations_confidences; - for (int i = 0; i < associations.size(); i++) { - if (associations(i) == -1) { - unfiltered_new_observations.conservativeResize(unfiltered_new_observations.size() + 2); - unfiltered_new_observations(unfiltered_new_observations.size() - 2) = - observations_global(i * 2); - unfiltered_new_observations(unfiltered_new_observations.size() - 1) = - observations_global(i * 2 + 1); - unfiltered_new_observations_confidences.conservativeResize( - unfiltered_new_observations_confidences.size() + 1); - unfiltered_new_observations_confidences(unfiltered_new_observations_confidences.size() - 1) = - observations_confidences(i); - associations(i) = -2; // Mark as not ready to be added to the graph - } - } - Eigen::VectorXd filtered_new_observations = this->_landmark_filter_->filter( - unfiltered_new_observations, unfiltered_new_observations_confidences); - if (!this->_graph_slam_instance_.new_pose_factors() && !this->_is_stopped_at_beginning_) { - return; - } - - if (this->_is_stopped_at_beginning_) { - for (int j = 0; j < associations.size(); j++) { - if (associations(j) >= 0) associations(j) = -2; - } - } + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Associations calculated"); - this->_landmark_filter_->delete_landmarks(filtered_new_observations); - if (this->_mission_ != common_lib::competition_logic::Mission::NONE && - !(this->_params_.using_preloaded_map_ && - (this->_mission_ == common_lib::competition_logic::Mission::SKIDPAD || - this->_mission_ == common_lib::competition_logic::Mission::ACCELERATION || - this->_mission_ == common_lib::competition_logic::Mission::AUTOCROSS || - this->_mission_ == common_lib::competition_logic::Mission::TRACKDRIVE)) && - lap_counter_ == 0) { - // Set the associations to -1 for the filtered observations - for (int i = 0; i < filtered_new_observations.size() / 2; i++) { - for (int j = 0; j < associations.size(); j++) { - if (std::hypot(filtered_new_observations(i * 2) - observations_global(j * 2), - filtered_new_observations(i * 2 + 1) - observations_global(j * 2 + 1)) < - this->_params_.data_association_gate_) { - associations(j) = -1; - break; - } - } - } - } + // Landmark filtering + Eigen::VectorXd filtered_new_observations = + this->_landmark_filter_->filter(observations_global, observations_confidences, associations); - Eigen::Vector3d pose; - pose << state(0), state(1), state(2); - - Eigen::VectorXi landmarks(state.size() - 3); - landmarks = state.segment(3, state.size() - 3).cast(); + // Loop closure detection LoopClosure::Result result = _loop_closure_->detect(pose, landmarks, associations, observations); if (result.detected) { lap_counter_++; // Create a (hard-ish) lock on the landmarks' positions after the first lap. if (lap_counter_ == 1) { - this->_graph_slam_instance_.lock_landmarks(0); + RCLCPP_INFO(rclcpp::get_logger("slam"), + "add_observations - First lap detected, locking landmarks"); + this->_graph_slam_instance_->lock_landmarks(0); } + RCLCPP_INFO(rclcpp::get_logger("slam"), "Lap counter: %d", lap_counter_); } + // Add observations to the graph { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "add_observations - Mutex locked - processing observations"); std::unique_lock uniq_lock(this->_mutex_); + ObservationData observation_data(std::make_shared(observations), std::make_shared(associations), std::make_shared(observations_global), @@ -196,18 +242,28 @@ void GraphSLAMSolver::add_observations(const std::vector_optimization_under_way_) { this->_observation_data_queue_.push(observation_data); + RCLCPP_INFO(rclcpp::get_logger("slam"), + "add_observations - Optimization under way, pushing observation data"); } - this->_graph_slam_instance_.process_observations(observation_data); + // Update the pose in the graph, as the vehicle might have moved since the last time a factor + // was added by the motion callback + this->_add_motion_data_to_graph(this->_pose_updater_, this->_graph_slam_instance_, true); + this->_graph_slam_instance_->process_observations(observation_data); + this->_landmark_filter_->delete_landmarks(filtered_new_observations); } factor_graph_time = rclcpp::Clock().now(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Mutex unlocked - Factors added"); - // Optimize the graph - if (this->_params_.slam_optimization_mode_ == "sync" && - !_is_stopped_at_beginning_) { // If optimization is synchronous + // Optimize the graph if optimization is synchronous + if (this->_params_.slam_optimization_mode_ == "sync-parallel") { + this->_asynchronous_optimization_routine(); + } else if (this->_params_.slam_optimization_mode_ == "sync") { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Mutex locked - optimizing graph"); std::unique_lock uniq_lock(this->_mutex_); - this->_graph_slam_instance_.optimize(); + this->_graph_slam_instance_->optimize(); optimization_time = rclcpp::Clock().now(); - this->_pose_updater_.set_last_pose(gtsam_pose_to_eigen(this->_graph_slam_instance_.get_pose())); + this->_pose_updater_->update_pose(gtsam_pose_to_eigen(this->_graph_slam_instance_->get_pose())); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "add_observations - Mutex unlocked - graph optimized"); } // Timekeeping @@ -222,46 +278,69 @@ void GraphSLAMSolver::add_observations(const std::vector_graph_slam_instance_.load_initial_state(map, pose, this->_params_.preloaded_map_noise_); +void GraphSLAMSolver::load_initial_state(const Eigen::VectorXd& map, const Eigen::Vector3d& pose) { + this->_graph_slam_instance_->load_initial_state(map, pose, this->_params_.preloaded_map_noise_); + this->_pose_updater_->update_pose(pose); // Update the pose in the pose updater } void GraphSLAMSolver::_asynchronous_optimization_routine() { rclcpp::Time start_time = rclcpp::Clock().now(); - GraphSLAMInstance graph_slam_instance_copy; - PoseUpdater pose_updater_copy; + + // Copy data to optimize + std::shared_ptr graph_slam_instance_copy; + std::shared_ptr pose_updater_copy; { const std::shared_lock lock(this->_mutex_); - if (!this->_graph_slam_instance_.new_observation_factors()) { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "_asynchronous_optimization_routine - Shared mutex accessed"); + if (!this->_graph_slam_instance_->new_observation_factors()) { return; } - graph_slam_instance_copy = this->_graph_slam_instance_; - pose_updater_copy = this->_pose_updater_; + graph_slam_instance_copy = this->_graph_slam_instance_->clone(); + pose_updater_copy = this->_pose_updater_->clone(); } { std::unique_lock lock(this->_mutex_); this->_optimization_under_way_ = true; } rclcpp::Time initialization_time = rclcpp::Clock().now(); + // Optimize the graph - graph_slam_instance_copy.optimize(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "_asynchronous_optimization_routine - Starting optimization"); + graph_slam_instance_copy->optimize(); + pose_updater_copy->update_pose(gtsam_pose_to_eigen(graph_slam_instance_copy->get_pose())); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "_asynchronous_optimization_routine - Graph optimized"); rclcpp::Time optimization_time = rclcpp::Clock().now(); + + // Rebuild the graph with the missed data { std::unique_lock lock(this->_mutex_); this->_optimization_under_way_ = false; + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "_asynchronous_optimization_routine - Mutex locked"); while (this->_motion_data_queue_.size() > 0 || this->_observation_data_queue_.size() > 0) { + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "_asynchronous_optimization_routine - Rebuilding graph, motion data queue size: " + "%ld, observation data queue size: %ld", + this->_motion_data_queue_.size(), this->_observation_data_queue_.size()); bool process_pose = this->_observation_data_queue_.size() <= 0 || (this->_motion_data_queue_.size() > 0 && this->_motion_data_queue_.front().timestamp_ < this->_observation_data_queue_.front().timestamp_); if (process_pose) { - auto [pose_difference, new_pose] = - pose_updater_copy.update_pose(this->_motion_data_queue_.front(), this->_motion_model_); - graph_slam_instance_copy.process_pose_difference(pose_difference, - new_pose); // Process the pose difference + std::shared_ptr motion_model_ptr = + this->_motion_data_queue_.front().type_ == MotionInputType::VELOCITIES + ? this->_motion_model_ + : this->_odometry_model; + pose_updater_copy->predict_pose(this->_motion_data_queue_.front(), + motion_model_ptr); // Predict the pose + this->_add_motion_data_to_graph(pose_updater_copy, graph_slam_instance_copy); this->_motion_data_queue_.pop(); } else { - graph_slam_instance_copy.process_observations(this->_observation_data_queue_.front()); + this->_add_motion_data_to_graph( + pose_updater_copy, + graph_slam_instance_copy); // To update graph pose before connecting factors + graph_slam_instance_copy->process_observations(this->_observation_data_queue_.front()); this->_observation_data_queue_.pop(); } } @@ -279,14 +358,18 @@ void GraphSLAMSolver::_asynchronous_optimization_routine() { this->_execution_times_->at(8) = (end_time - start_time).seconds() * 1000.0; // Optimization time } + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "_asynchronous_optimization_routine - Mutex unlocked - Graph updated"); } std::vector -GraphSLAMSolver::get_map_estimate() { // TODO: include the map updates in observation to save time +GraphSLAMSolver::get_map_estimate() { // TODO: include the map updates in observation to save + // time + const std::shared_lock lock(this->_mutex_); rclcpp::Time current_time = rclcpp::Clock().now(); - const gtsam::Values& graph_values = this->_graph_slam_instance_.get_graph_values_reference(); + const gtsam::Values& graph_values = this->_graph_slam_instance_->get_graph_values_reference(); std::vector map_estimate; - map_estimate.reserve(this->_graph_slam_instance_.get_landmark_counter()); + map_estimate.reserve(this->_graph_slam_instance_->get_landmark_counter()); // const gtsam::Marginals marginals(this->_factor_graph_, this->_graph_values_); for (auto it = graph_values.begin(); it != graph_values.end(); ++it) { // Iterate through the landmarks in the _graph_values_ @@ -303,6 +386,9 @@ GraphSLAMSolver::get_map_estimate() { // TODO: include the map updates in obser current_time)); // TODO: vary cone confidence } } + rclcpp::Time end_time = rclcpp::Clock().now(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "get_map_estimate - Time: %f ms", + (end_time - current_time).seconds() * 1000.0); return map_estimate; } @@ -310,21 +396,44 @@ common_lib::structures::Pose GraphSLAMSolver::get_pose_estimate() { // const gtsam::Marginals marginals(this->_factor_graph_, this->_graph_values_); // const gtsam::Key key = gtsam::Symbol('x', this->_pose_counter_); // const gtsam::Matrix covariance = marginals.marginalCovariance(key); - - Eigen::Vector3d pose_vector = this->_pose_updater_.get_last_pose(); + const std::shared_lock lock(this->_mutex_); + Eigen::Vector3d pose_vector = this->_pose_updater_->get_last_pose(); return common_lib::structures::Pose(pose_vector(0), pose_vector(1), pose_vector(2), 0.0, 0.0, 0.0, - this->_pose_updater_.get_last_pose_update()); + this->_pose_updater_->get_last_pose_update()); +} + +std::vector GraphSLAMSolver::get_trajectory_estimate() { + const std::shared_lock lock(this->_mutex_); + std::vector trajectory; + const gtsam::Values& graph_values = this->_graph_slam_instance_->get_graph_values_reference(); + trajectory.reserve(this->_graph_slam_instance_->get_pose_counter()); + for (auto it = graph_values.begin(); it != graph_values.end(); ++it) { + // Iterate through the poses in the _graph_values_ + if (gtsam::Symbol(it->key).chr() == 'x') { + gtsam::Pose2 pose = it->value.cast(); + trajectory.push_back(common_lib::structures::Pose(pose.x(), pose.y(), pose.theta())); + } + } + return trajectory; } Eigen::MatrixXd GraphSLAMSolver::get_covariance() { - return this->_graph_slam_instance_.get_covariance_matrix(); + const std::shared_lock lock(this->_mutex_); + return this->_graph_slam_instance_->get_covariance_matrix(); } -Eigen::VectorXi GraphSLAMSolver::get_associations() const { return this->_associations_; } +Eigen::VectorXi GraphSLAMSolver::get_associations() const { + const std::shared_lock lock(this->_mutex_); + return this->_associations_; +} Eigen::VectorXd GraphSLAMSolver::get_observations_global() const { + const std::shared_lock lock(this->_mutex_); return this->_observations_global_; } -Eigen::VectorXd GraphSLAMSolver::get_map_coordinates() const { return this->_map_coordinates_; } \ No newline at end of file +Eigen::VectorXd GraphSLAMSolver::get_map_coordinates() const { + const std::shared_lock lock(this->_mutex_); + return this->_map_coordinates_; +} \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp b/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp index f052348d7..e0e5c86c4 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp @@ -1,9 +1,19 @@ #include "slam_solver/graph_slam_solver/optimizer/isam2_optimizer.hpp" +#include +#include #include #include +#include +#include +#include +#include +#include #include +#include +#include +#include ISAM2Optimizer::ISAM2Optimizer(const SLAMParameters& params) : BaseOptimizer(params) { gtsam::ISAM2Params isam_params; @@ -14,22 +24,45 @@ ISAM2Optimizer::ISAM2Optimizer(const SLAMParameters& params) : BaseOptimizer(par : gtsam::ISAM2Params::CHOLESKY; _isam2_ = gtsam::ISAM2(isam_params); _last_estimate_ = gtsam::Values(); + _new_factors_ = gtsam::NonlinearFactorGraph(); + _new_values_ = gtsam::Values(); }; -gtsam::Values ISAM2Optimizer::optimize(gtsam::NonlinearFactorGraph& factor_graph, - gtsam::Values& graph_values, +ISAM2Optimizer::ISAM2Optimizer(const ISAM2Optimizer& other) : BaseOptimizer(other) { + this->_isam2_ = other._isam2_; + this->_last_estimate_ = other._last_estimate_; + this->_new_values_ = other._new_values_; + this->_new_factors_ = other._new_factors_; +} + +ISAM2Optimizer& ISAM2Optimizer::operator=(const ISAM2Optimizer& other) { + if (this == &other) return *this; // Prevent self-assignment + + // Copy each member individually + BaseOptimizer::operator=(other); + this->_isam2_ = other._isam2_; + this->_last_estimate_ = other._last_estimate_; + this->_new_values_ = other._new_values_; + this->_new_factors_ = other._new_factors_; + + return *this; +} + +std::shared_ptr ISAM2Optimizer::clone() const { + return std::make_shared(*this); +} + +gtsam::Values ISAM2Optimizer::optimize([[maybe_unused]] gtsam::NonlinearFactorGraph& factor_graph, + [[maybe_unused]] gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, [[maybe_unused]] unsigned int landmark_num) { - // Remove values from graph_values that are in _last_estimate_ - gtsam::Values new_values; - for (const auto& key : graph_values.keys()) { - if (!_last_estimate_.exists(key)) { - new_values.insert(key, graph_values.at(key)); - } - } - - _isam2_.update(factor_graph, new_values); - factor_graph.resize(0); // Clear the factor graph + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "ISAM2Optimizer - Adding %zu new factors", + factor_graph.size()); + _isam2_.update(this->_new_factors_, this->_new_values_); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "ISAM2Optimizer - Updating ISAM2"); + this->_new_factors_.resize(0); + this->_new_values_.clear(); _last_estimate_ = _isam2_.calculateEstimate(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "ISAM2Optimizer - Optimization complete"); return _last_estimate_; } \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.cpp b/src/slam/src/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.cpp index faf43e95e..029f36592 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/optimizer/normal_levenberg_optimizer.cpp @@ -3,6 +3,10 @@ NormalLevenbergOptimizer::NormalLevenbergOptimizer(const SLAMParameters& params) : BaseOptimizer(params){}; +std::shared_ptr NormalLevenbergOptimizer::clone() const { + return std::make_shared(*this); +} + gtsam::Values NormalLevenbergOptimizer::optimize(gtsam::NonlinearFactorGraph& factor_graph, gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, diff --git a/src/slam/src/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.cpp b/src/slam/src/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.cpp index 1a477e7a0..2fcc5bdf1 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.cpp @@ -1,6 +1,16 @@ #include "slam_solver/graph_slam_solver/optimizer/sliding_window_levenberg_optimizer.hpp" +#include #include +#include +#include +#include + +#include + +std::shared_ptr SlidingWindowLevenbergOptimizer::clone() const { + return std::make_shared(*this); +} SlidingWindowLevenbergOptimizer::SlidingWindowLevenbergOptimizer(const SLAMParameters& params) : BaseOptimizer(params){}; @@ -8,13 +18,14 @@ SlidingWindowLevenbergOptimizer::SlidingWindowLevenbergOptimizer(const SLAMParam gtsam::Values SlidingWindowLevenbergOptimizer::optimize( gtsam::NonlinearFactorGraph& factor_graph, gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, [[maybe_unused]] unsigned int landmark_num) { - // Retrieve the last 5 poses added to the graph - std::unordered_set relevant_keys; + // Retrieve the last poses added to the graph + std::set pose_keys; + std::set landmark_keys; gtsam::NonlinearFactorGraph sliding_window_graph; gtsam::Values sliding_window_values; for (int i = std::max(1, pose_num - _params_.sliding_window_size_ + 1); i <= static_cast(pose_num); ++i) { - relevant_keys.insert(gtsam::Symbol('x', i)); + pose_keys.insert(gtsam::Symbol('x', i)); } // Add relevant factors @@ -22,12 +33,12 @@ gtsam::Values SlidingWindowLevenbergOptimizer::optimize( auto involved_keys = factor->keys(); bool include = false; for (const auto& key : involved_keys) { - if (gtsam::Symbol(key).chr() == 'x' && relevant_keys.count(key) == 0) { + if (gtsam::Symbol(key).chr() == 'x' && pose_keys.count(key) == 0) { // If one of the involved nodes in the factor is a pose // and not one in the sliding window, ignore the factor include = false; break; - } else if (gtsam::Symbol(key).chr() == 'x' && relevant_keys.count(key) > 0) { + } else if (gtsam::Symbol(key).chr() == 'x' && pose_keys.count(key) > 0) { // If one of the involved nodes in the factor is a pose // and is in the sliding window, include the factor, but // wait to check if the other node is a pose and not in the @@ -45,7 +56,7 @@ gtsam::Values SlidingWindowLevenbergOptimizer::optimize( auto involved_keys = factor->keys(); for (const auto& key : involved_keys) { if (gtsam::Symbol(key).chr() == 'l') { - relevant_keys.insert(key); + landmark_keys.insert(key); } } } @@ -57,19 +68,58 @@ gtsam::Values SlidingWindowLevenbergOptimizer::optimize( // Check if this is a prior factor on a landmark in the current window if (keys.size() == 1) { gtsam::Symbol symbol(keys[0]); - if (symbol.chr() == 'l' && relevant_keys.count(keys[0]) > 0) { + if (symbol.chr() == 'l' && landmark_keys.count(keys[0]) > 0) { sliding_window_graph.add(factor); } } } // Add relevant values - for (const auto& key : relevant_keys) { + for (const auto& key : landmark_keys) { + sliding_window_values.insert(key, graph_values.at(key)); + } + for (const auto& key : pose_keys) { sliding_window_values.insert(key, graph_values.at(key)); } + // Define parameters + gtsam::LevenbergMarquardtParams lm_params; + gtsam::Ordering ordering; + for (const auto& key : landmark_keys) { + ordering.push_back(key); + } + for (const auto& key : pose_keys) { + ordering.push_back(key); + } + lm_params.setOrdering(ordering); + // lm_params.setMaxIterations(2); // steady latency + // lm_params.setlambdaInitial(1e-3); // sane start + lm_params.setlambdaUpperBound(1e3); // clamp runaway damping + // lm_params.setlambdaFactor(10.0); // backoff multiplier + // lm_params.setAbsoluteErrorTol(0); // disable early-stopping by tolerance (latency focus) + // lm_params.setRelativeErrorTol(0); // " + // lm_params.setLinearSolverType(gtsam::LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY); + // lm_params.setOrderingType( + // gtsam::LevenbergMarquardtParams::OrderingType::CUSTOM); // supply your ordering + // TODO: determine best parameters and expose them to YAML + // Optimize the sliding window - gtsam::LevenbergMarquardtOptimizer optimizer(sliding_window_graph, sliding_window_values); + rclcpp::Time start_time = rclcpp::Clock().now(); + gtsam::LevenbergMarquardtOptimizer optimizer(sliding_window_graph, sliding_window_values, + lm_params); graph_values.update(optimizer.optimize()); + rclcpp::Duration optimization_time = rclcpp::Clock().now() - start_time; + RCLCPP_DEBUG( + rclcpp::get_logger("slam"), + "SlidingWindowLevenbergOptimizer - Optimized %d poses and %d landmarks, including %d factors", + static_cast(pose_keys.size()), static_cast(landmark_keys.size()), + static_cast(sliding_window_graph.size())); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "SlidingWindowLevenbergOptimizer - Lambda at start: %lf, end: %lf", + lm_params.lambdaInitial, optimizer.lambda()); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), + "SlidingWindowLevenbergOptimizer - Number of iterations: " + "%ld\nOptimization time: %f", + optimizer.iterations(), optimization_time.seconds()); return graph_values; } \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater.cpp deleted file mode 100644 index 5b09f5515..000000000 --- a/src/slam/src/slam_solver/graph_slam_solver/pose_updater.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "slam_solver/graph_slam_solver/pose_updater.hpp" - -std::pair PoseUpdater::update_pose( - const MotionData& motion_data, std::shared_ptr motion_model) { - if (!this->_received_first_velocities_) { - this->_last_pose_ = Eigen::Vector3d(0.0, 0.0, 0.0); - this->_last_pose_update_ = motion_data.timestamp_; - this->_received_first_velocities_ = true; - return {Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}; - } - - double delta = (motion_data.timestamp_ - this->_last_pose_update_).nanoseconds() / 1e9; - Eigen::Vector3d pose_difference = - motion_model->get_pose_difference(this->_last_pose_, *(motion_data.velocities_), delta); - - this->_last_pose_ = - motion_model->get_next_pose(this->_last_pose_, *(motion_data.velocities_), delta); - this->_last_pose_update_ = motion_data.timestamp_; - - return {pose_difference, this->_last_pose_}; -} - -PoseUpdater::PoseUpdater(const PoseUpdater& other) { - this->_last_pose_ = other._last_pose_; - this->_last_pose_update_ = other._last_pose_update_; - this->_received_first_velocities_ = other._received_first_velocities_; -} - -PoseUpdater& PoseUpdater::operator=(const PoseUpdater& other) { - if (this == &other) return *this; // Prevent self-assignment - - // Copy each member individually - this->_last_pose_ = other._last_pose_; - this->_last_pose_update_ = other._last_pose_update_; - this->_received_first_velocities_ = other._received_first_velocities_; - - return *this; -} \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp new file mode 100644 index 000000000..ca737c7e0 --- /dev/null +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp @@ -0,0 +1,110 @@ +#include "slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp" + +#include "slam_solver/graph_slam_solver/utils.hpp" + +PoseUpdater::PoseUpdater([[maybe_unused]] const SLAMParameters& params) { + // Initialize the pose updater with parameters from SLAMParameters + this->_last_pose_ = Eigen::Vector3d::Zero(); + this->_last_graphed_pose_ = Eigen::Vector3d::Zero(); + this->_received_first_motion_data_ = false; + this->_last_pose_update_ = rclcpp::Time(0); + this->_new_pose_from_graph_ = false; + this->_last_pose_covariance_ = Eigen::Matrix3d::Zero(); +} + +PoseUpdater::PoseUpdater(const PoseUpdater& other) { + this->_last_pose_ = other._last_pose_; + this->_last_graphed_pose_ = other._last_graphed_pose_; + this->_last_pose_update_ = other._last_pose_update_; + this->_new_pose_from_graph_ = other._new_pose_from_graph_; + this->_last_pose_covariance_ = other._last_pose_covariance_; + this->_received_first_motion_data_ = other._received_first_motion_data_; +} + +PoseUpdater::~PoseUpdater() = default; + +PoseUpdater& PoseUpdater::operator=(const PoseUpdater& other) { + if (this == &other) return *this; // Prevent self-assignment + + // Copy each member individually + this->_last_pose_ = other._last_pose_; + this->_last_pose_update_ = other._last_pose_update_; + this->_last_graphed_pose_ = other._last_graphed_pose_; + this->_last_pose_covariance_ = other._last_pose_covariance_; + this->_new_pose_from_graph_ = other._new_pose_from_graph_; + this->_received_first_motion_data_ = other._received_first_motion_data_; + + return *this; +} + +std::shared_ptr PoseUpdater::clone() const { + return std::make_shared(*this); +} + +void PoseUpdater::update_pose(const Eigen::Vector3d& last_pose) { + this->_last_pose_ = last_pose; + this->_last_graphed_pose_ = last_pose; + this->_last_pose_covariance_ = Eigen::Matrix3d::Zero(); + this->_new_pose_from_graph_ = false; // Reset the flag for new pose from graph +} + +void PoseUpdater::predict_pose(const MotionData& motion_data, + std::shared_ptr motion_model) { + if (!this->_received_first_motion_data_) { + this->_last_pose_ = Eigen::Vector3d(0.0, 0.0, 0.0); + this->_last_pose_update_ = motion_data.timestamp_; + this->_received_first_motion_data_ = true; + return; + } + this->_last_pose_update_.seconds(), motion_data.timestamp_.seconds(); + double delta = (motion_data.timestamp_ - this->_last_pose_update_).seconds(); + RCLCPP_DEBUG(rclcpp::get_logger("slam"), "Delta time: %f", delta); + Eigen::Vector3d new_pose = + motion_model->get_next_pose(this->_last_pose_, *(motion_data.motion_data_), delta); + Eigen::MatrixXd jacobian_motion_data = + motion_model->get_jacobian_motion_data(this->_last_pose_, *(motion_data.motion_data_), delta); + Eigen::MatrixXd motion_noise = jacobian_motion_data * + motion_data.motion_data_noise_->asDiagonal() * + jacobian_motion_data.transpose(); + Eigen::Vector3d pose_difference = pose_difference_eigen(this->_last_pose_, new_pose); + Eigen::Matrix3d adjoint_matrix = + this->get_adjoint_operator_matrix(pose_difference(0), pose_difference(1), pose_difference(2)); + + this->_last_pose_ = new_pose; + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Last Pose Covariance: \n" + // << this->_last_pose_covariance_); + this->_last_pose_covariance_ = + adjoint_matrix * this->_last_pose_covariance_ * adjoint_matrix.transpose() + + motion_noise; // TODO: to improve further, consider including a noise for the intrinsic error + // of the motion model + this->_last_pose_update_ = motion_data.timestamp_; + this->_new_pose_from_graph_ = true; + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Transposed Jacobian Motion Data: \n" + // << jacobian_motion_data.transpose()); + // RCLCPP_DEBUG_STREAM( + // rclcpp::get_logger("slam"), + // "Motion Data Noise Diagonal: \n" + // << static_cast(motion_data.motion_data_noise_->asDiagonal())); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Adjoint Matrix: \n" << adjoint_matrix); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Motion Noise: \n" << motion_noise); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Jacobian Motion Data: \n" + // << jacobian_motion_data); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Pose Difference Covariance: \n" + // << this->_last_pose_covariance_); +} + +bool PoseUpdater::pose_ready_for_graph_update() const { return _new_pose_from_graph_; } + +Eigen::Matrix3d PoseUpdater::get_adjoint_operator_matrix(const double x_translation, + const double y_translation, + const double rotation_angle) const { + Eigen::Matrix3d adjoint_matrix = Eigen::Matrix3d::Zero(); + adjoint_matrix(0, 0) = cos(rotation_angle); + adjoint_matrix(0, 1) = -sin(rotation_angle); + adjoint_matrix(0, 2) = y_translation; + adjoint_matrix(1, 0) = sin(rotation_angle); + adjoint_matrix(1, 1) = cos(rotation_angle); + adjoint_matrix(1, 2) = -x_translation; + adjoint_matrix(2, 2) = 1.0; + return adjoint_matrix; +} \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.cpp new file mode 100644 index 000000000..c8c5047a3 --- /dev/null +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.cpp @@ -0,0 +1,39 @@ +#include "slam_solver/graph_slam_solver/pose_updater/difference_based_ready_pose_updater.hpp" + +#include "slam_solver/graph_slam_solver/utils.hpp" + +DifferenceBasedReadyPoseUpdater::DifferenceBasedReadyPoseUpdater(const SLAMParameters& params) + : PoseUpdater(params) { + this->minimum_pose_difference_ = params.slam_min_pose_difference_; +} + +DifferenceBasedReadyPoseUpdater::DifferenceBasedReadyPoseUpdater( + const DifferenceBasedReadyPoseUpdater& other) + : PoseUpdater(other) { + this->minimum_pose_difference_ = other.minimum_pose_difference_; +} + +DifferenceBasedReadyPoseUpdater::~DifferenceBasedReadyPoseUpdater() = default; + +DifferenceBasedReadyPoseUpdater& DifferenceBasedReadyPoseUpdater::operator=( + const DifferenceBasedReadyPoseUpdater& other) { + if (this == &other) return *this; // Prevent self-assignment + + // Copy each member individually + PoseUpdater::operator=(other); // Call base class assignment operator + this->minimum_pose_difference_ = other.minimum_pose_difference_; + + return *this; +} + +std::shared_ptr DifferenceBasedReadyPoseUpdater::clone() const { + return std::make_shared(*this); +} + +bool DifferenceBasedReadyPoseUpdater::pose_ready_for_graph_update() const { + Eigen::Vector3d pose_difference = + pose_difference_eigen(this->_last_graphed_pose_, this->_last_pose_); + double pose_difference_norm = + ::sqrt(pow(pose_difference(0), 2) + pow(pose_difference(1), 2) + pow(pose_difference(2), 2)); + return pose_difference_norm >= this->minimum_pose_difference_; +} diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.cpp new file mode 100644 index 000000000..086952bd0 --- /dev/null +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/double_pose_updater.cpp @@ -0,0 +1,45 @@ +#include "slam_solver/graph_slam_solver/pose_updater/double_pose_updater.hpp" + +DoublePoseUpdater::DoublePoseUpdater(const SLAMParameters& params) + : DifferenceBasedReadyPoseUpdater(params) { + this->_received_first_velocities_ = false; + this->_received_first_odometry_ = false; + this->_accumulated_odometry_pose_difference_ = Eigen::Vector3d::Zero(); + this->_last_odometry_pose_ = Eigen::Vector3d::Zero(); + this->_last_velocities_receive_time_ = + rclcpp::Time(0); // Initialize the last velocities receive time +} + +DoublePoseUpdater::DoublePoseUpdater(const DoublePoseUpdater& other) + : DifferenceBasedReadyPoseUpdater(other) { + this->_received_first_velocities_ = other._received_first_velocities_; + this->_received_first_odometry_ = other._received_first_odometry_; + this->_last_velocities_receive_time_ = other._last_velocities_receive_time_; + this->_accumulated_odometry_pose_difference_ = other._accumulated_odometry_pose_difference_; + this->_last_odometry_pose_ = other._last_odometry_pose_; +} + +DoublePoseUpdater& DoublePoseUpdater::operator=(const DoublePoseUpdater& other) { + if (this == &other) return *this; // Prevent self-assignment + + // Copy each member individually + DifferenceBasedReadyPoseUpdater::operator=(other); // Call base class assignment operator + this->_received_first_velocities_ = other._received_first_velocities_; + this->_received_first_odometry_ = other._received_first_odometry_; + this->_last_velocities_receive_time_ = other._last_velocities_receive_time_; + this->_accumulated_odometry_pose_difference_ = other._accumulated_odometry_pose_difference_; + this->_last_odometry_pose_ = other._last_odometry_pose_; + + return *this; +} + +void DoublePoseUpdater::predict_pose(const MotionData& motion_data, + std::shared_ptr motion_model) {} + +bool DoublePoseUpdater::second_pose_difference_ready() const { + return true; // TODO: implement this shit +} + +Eigen::Vector3d DoublePoseUpdater::get_second_accumulated_pose_difference() const { + return _accumulated_odometry_pose_difference_; +} \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/utils.cpp b/src/slam/src/slam_solver/graph_slam_solver/utils.cpp index 0b7ecdcca..b8eed914c 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/utils.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/utils.cpp @@ -6,4 +6,11 @@ gtsam::Pose2 eigen_to_gtsam_pose(const Eigen::Vector3d& pose) { Eigen::Vector3d gtsam_pose_to_eigen(const gtsam::Pose2& pose) { return Eigen::Vector3d(pose.x(), pose.y(), pose.theta()); +} + +Eigen::Vector3d pose_difference_eigen(const Eigen::Vector3d& pose1, const Eigen::Vector3d& pose2) { + gtsam::Pose2 gtsam_pose1 = eigen_to_gtsam_pose(pose1); + gtsam::Pose2 gtsam_pose2 = eigen_to_gtsam_pose(pose2); + gtsam::Pose2 gtsam_difference = gtsam_pose1.between(gtsam_pose2); + return gtsam_pose_to_eigen(gtsam_difference); } \ No newline at end of file diff --git a/src/slam/src/slam_solver/slam_solver.cpp b/src/slam/src/slam_solver/slam_solver.cpp index d61e949dd..b3bb32c3f 100644 --- a/src/slam/src/slam_solver/slam_solver.cpp +++ b/src/slam/src/slam_solver/slam_solver.cpp @@ -18,9 +18,7 @@ void SLAMSolver::set_mission(common_lib::competition_logic::Mission mission) { this->_mission_ = mission; if (previous_mission_ == common_lib::competition_logic::Mission::NONE && (this->_mission_ == common_lib::competition_logic::Mission::SKIDPAD || - this->_mission_ == common_lib::competition_logic::Mission::ACCELERATION || - this->_mission_ == common_lib::competition_logic::Mission::TRACKDRIVE || - this->_mission_ == common_lib::competition_logic::Mission::AUTOCROSS) && + this->_mission_ == common_lib::competition_logic::Mission::ACCELERATION) && this->_params_.using_preloaded_map_) { if (_mission_ == common_lib::competition_logic::Mission::SKIDPAD) { Eigen::Vector3d pose; @@ -32,12 +30,6 @@ void SLAMSolver::set_mission(common_lib::competition_logic::Mission mission) { Eigen::VectorXd map; load_acceleration_track(pose, map); this->load_initial_state(map, pose); - } else if (_mission_ == common_lib::competition_logic::Mission::TRACKDRIVE || - _mission_ == common_lib::competition_logic::Mission::AUTOCROSS) { - Eigen::Vector3d pose; - Eigen::VectorXd map; - load_trackdrive_track(pose, map); - this->load_initial_state(map, pose); } } } diff --git a/src/slam/src/track_loader/track_loader.cpp b/src/slam/src/track_loader/track_loader.cpp index 4ec21f794..0c7dddf4a 100644 --- a/src/slam/src/track_loader/track_loader.cpp +++ b/src/slam/src/track_loader/track_loader.cpp @@ -51,13 +51,6 @@ void load_acceleration_track(Eigen::Vector3d& start_pose, Eigen::VectorXd& track transform_track(track, start_pose); } -void load_trackdrive_track(Eigen::Vector3d& start_pose, Eigen::VectorXd& track) { - std::string package_prefix = ament_index_cpp::get_package_prefix("slam"); - std::string mapPath = package_prefix + "/../../src/slam/maps/trackdrive.yaml"; - load_initial_state(mapPath, start_pose, track); - transform_track(track, start_pose); -} - void load_skidpad_track(Eigen::Vector3d& start_pose, Eigen::VectorXd& track) { std::string package_prefix = ament_index_cpp::get_package_prefix("slam"); std::string mapPath = package_prefix + "/../../src/slam/maps/skidpad.yaml"; diff --git a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp index 0e26efb85..701a2bb5c 100644 --- a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp +++ b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp @@ -15,7 +15,8 @@ class MockDataAssociationModel : public DataAssociationModel { class MockLandmarkFilter : public LandmarkFilter { public: MOCK_METHOD(Eigen::VectorXd, filter, - (const Eigen::VectorXd& observations, const Eigen::VectorXd& observation_confidences), + (const Eigen::VectorXd& observations, const Eigen::VectorXd& observation_confidences, + Eigen::VectorXi& associations), (override)); MOCK_METHOD(void, delete_landmarks, (const Eigen::VectorXd& some_landmarks), (override)); }; @@ -23,17 +24,17 @@ class MockLandmarkFilter : public LandmarkFilter { class MockV2PModel : public V2PMotionModel { public: MOCK_METHOD(Eigen::Vector3d, get_pose_difference, - (const Eigen::Vector3d& previous_pose, const Eigen::Vector3d& velocities, + (const Eigen::Vector3d& previous_pose, const Eigen::VectorXd& motion_data, double delta_t), (override)); MOCK_METHOD(Eigen::Matrix3d, get_jacobian_pose, - (const Eigen::Vector3d& previous_pose, const Eigen::Vector3d& velocities, + (const Eigen::Vector3d& previous_pose, const Eigen::VectorXd& motion_data, const double delta_t), (override)); - MOCK_METHOD(Eigen::Matrix3d, get_jacobian_velocities, - (const Eigen::Vector3d& previous_pose, const Eigen::Vector3d& velocities, + MOCK_METHOD(Eigen::MatrixXd, get_jacobian_motion_data, + (const Eigen::Vector3d& previous_pose, const Eigen::VectorXd& motion_data, const double delta_t), (override)); }; @@ -41,7 +42,7 @@ class MockV2PModel : public V2PMotionModel { class MockLoopClosure : public LoopClosure { public: MOCK_METHOD(LoopClosure::Result, detect, - (const Eigen::Vector3d& current_pose, const Eigen::VectorXi& map_cones, + (const Eigen::Vector3d& current_pose, const Eigen::VectorXd& map_cones, const Eigen::VectorXi& associations, const Eigen::VectorXd& observations), (const, override)); }; @@ -88,7 +89,7 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { EXPECT_CALL(*mock_motion_model_ptr, get_pose_difference) .Times(8) .WillRepeatedly(testing::Return(Eigen::Vector3d(1.1, 0.0, 0.0))); - // EXPECT_CALL(*mock_motion_model_ptr, get_jacobian_velocities) + // EXPECT_CALL(*mock_motion_model_ptr, get_jacobian_motion_data) // .Times(4) // .WillRepeatedly(testing::Return(Eigen::Matrix3d::Identity() * 0.1)); EXPECT_CALL(*mock_data_association_ptr, associate) @@ -133,15 +134,15 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { // Act solver->add_observations(cones_start); - solver->add_motion_prior(velocities); + solver->add_velocities(velocities); velocities.timestamp_ += rclcpp::Duration(1, 0); - solver->add_motion_prior(velocities); + solver->add_velocities(velocities); velocities.timestamp_ += rclcpp::Duration(1, 0); - solver->add_motion_prior(velocities); + solver->add_velocities(velocities); velocities.timestamp_ += rclcpp::Duration(1, 0); - solver->add_motion_prior(velocities); + solver->add_velocities(velocities); velocities.timestamp_ += rclcpp::Duration(1, 0); - solver->add_motion_prior(velocities); + solver->add_velocities(velocities); const common_lib::structures::Pose pose_before_observations = solver->get_pose_estimate(); const std::vector map_before_observations = solver->get_map_estimate(); From 312a6990374c873933a1f60a0bda06028b1bd1f9 Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Wed, 5 Nov 2025 22:14:58 +0000 Subject: [PATCH 02/10] publishing --- config/slam/vehicle.yaml | 2 +- src/perception_sensor_lib/CMakeLists.txt | 12 +- .../slam_base_observation_model.hpp} | 6 +- .../observation_model/ve/map.hpp | 24 +++ .../ve/no_slip_bicycle_model.hpp | 29 +++ .../ve/ve_base_observation_model.hpp | 43 ++++ .../slam_base_observation_model.cpp} | 12 +- .../ve/no_slip_bicycle_model.cpp | 100 +++++++++ .../slam_base_observation_model_test.cpp} | 20 +- .../ve/no_slip_bicycle_model_test.cpp | 202 ++++++++++++++++++ .../include/slam_solver/ekf_slam_solver.hpp | 4 +- src/slam/src/slam_solver/ekf_slam_solver.cpp | 2 +- src/velocity_estimation/CMakeLists.txt | 3 + .../include/estimators/ekf.hpp | 2 +- .../include/estimators/no_rear_wss_ekf.hpp | 2 +- 15 files changed, 435 insertions(+), 28 deletions(-) rename src/perception_sensor_lib/include/perception_sensor_lib/observation_model/{base_observation_model.hpp => slam/slam_base_observation_model.hpp} (96%) create mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/map.hpp create mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp create mode 100644 src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/ve_base_observation_model.hpp rename src/perception_sensor_lib/src/observation_model/{base_observation_model.cpp => slam/slam_base_observation_model.cpp} (87%) create mode 100644 src/perception_sensor_lib/src/observation_model/ve/no_slip_bicycle_model.cpp rename src/perception_sensor_lib/test/observation_model/{base_observation_model_test.cpp => slam/slam_base_observation_model_test.cpp} (74%) create mode 100644 src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index 35981fd5b..959ea6ce1 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -22,7 +22,7 @@ slam: slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver slam_optimization_mode: "async" # "sync" or "sync-parallel" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step and sync-parallel is sync but callbacks execute in parallel slam_optimization_in_poses: false # If true, the optimization will be done also when poses are added, not only when observations are added - slam_optimization_type: "isam2" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type + slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type # Only for pose_updater_name: "difference_based_ready" slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph diff --git a/src/perception_sensor_lib/CMakeLists.txt b/src/perception_sensor_lib/CMakeLists.txt index 951843aed..7e22291e8 100644 --- a/src/perception_sensor_lib/CMakeLists.txt +++ b/src/perception_sensor_lib/CMakeLists.txt @@ -23,7 +23,11 @@ foreach(pkg ${PACKAGES}) endforeach() # Find PCL -find_package(PCL REQUIRED COMPONENTS registration common io) +find_package(PCL COMPONENTS registration common io QUIET) +if(NOT PCL_FOUND) + message(WARNING "PCL not found; building perception_sensor_lib without PCL-dependent features") +else() +endif() # Enable OpenMP (optional) find_package(OpenMP) @@ -37,7 +41,8 @@ set(IMPLEMENTATION_FILES src/data_association/nearest_neighbour_icp.cpp src/data_association/nearest_neighbor.cpp src/data_association/jcbb.cpp - src/observation_model/base_observation_model.cpp + src/observation_model/slam/slam_base_observation_model.cpp + src/observation_model/ve/no_slip_bicycle_model.cpp src/landmark_filter/consecutive_counter_filter.cpp src/loop_closure/lap_counter.cpp ) @@ -78,7 +83,8 @@ if(BUILD_TESTING) set(TESTFILES test/main.cpp - test/observation_model/base_observation_model_test.cpp + test/observation_model/slam/slam_base_observation_model_test.cpp + test/observation_model/ve/no_slip_bicycle_model_test.cpp test/data_association/maximum_likelihood_nll_test.cpp test/data_association/nearest_neighbor_test.cpp test/data_association/nearest_neighbour_test.cpp diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/base_observation_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp similarity index 96% rename from src/perception_sensor_lib/include/perception_sensor_lib/observation_model/base_observation_model.hpp rename to src/perception_sensor_lib/include/perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp index 95ba410a7..fed009d55 100644 --- a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/base_observation_model.hpp +++ b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp @@ -4,10 +4,10 @@ #include #include "common_lib/maths/transformations.hpp" -class ObservationModel { +class SLAMObservationModel { public: - ObservationModel() = default; - virtual ~ObservationModel() = default; + SLAMObservationModel() = default; + virtual ~SLAMObservationModel() = default; /** * @brief transform landmarks' positions from global frame to the car's frame diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/map.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/map.hpp new file mode 100644 index 000000000..5c21a7d81 --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/map.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +#include "no_slip_bicycle_model.hpp" + +/* + * Map of Velocity estimation observation models, with the key being the name of the model and the + * value being a lambda function that returns a shared pointer to the corresponding observation + * model. + */ +const std::map( + const common_lib::car_parameters::CarParameters&)>, + std::less<>> + ve_observation_models_map = { + {"no_slip_bicycle_model", + [](const common_lib::car_parameters::CarParameters& params) + -> std::shared_ptr { + return std::make_shared(params); + }}, +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp new file mode 100644 index 000000000..dc23986df --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include "ve_base_observation_model.hpp" + +class NoSlipBicycleModel : public VEObservationModel { +public: + NoSlipBicycleModel(const common_lib::car_parameters::CarParameters& car_parameters) + : VEObservationModel(car_parameters) {} + + /** + * @brief + * + * @param state velocities at the center of mass of the vehicle, in its body frame. + * The state vector must have the following structure: [vx, vy, angular velocity]. + * + * @return Eigen::VectorXd Expected WSS, SAS and Motor Resolver Readings with the following + * structure: [WSS_FL, WSS_FR, WSS_RL, WSS_RR, SAS, Motor Resolver]. + * WSS and Motor resolver readings are in RPMs, SAS is in radians. + */ + Eigen::VectorXd expected_observations(const Eigen::VectorXd& state) const override; + + /** + * @brief calculates the Jacobian of the expected_observations function with respect to the state. + * + * @param state velocities at the CG + * @return Eigen::MatrixXd of size (6, 3) where 6 is the number of expected observations + */ + Eigen::MatrixXd expected_observations_jacobian(const Eigen::VectorXd& state) const override; +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/ve_base_observation_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/ve_base_observation_model.hpp new file mode 100644 index 000000000..98bd1914b --- /dev/null +++ b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/ve_base_observation_model.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include "common_lib/car_parameters/car_parameters.hpp" + +/** + * @brief Base class for velocity estimation observation models. + * + */ +class VEObservationModel { +protected: + std::shared_ptr car_parameters_; + +public: + VEObservationModel(const common_lib::car_parameters::CarParameters& car_parameters) + : car_parameters_( + std::make_shared(car_parameters)) {} + + /** + * @brief Calculates expected observations based on the state. The state vector can contain + * variable information about the vehicle's dynamic state, such as its velocity or acceleration. + * The expected observations are the sensor readings that the model predicts based on the current + * state of the vehicle, and can vary depending on the type of sensor being used. + * + * @param state The state vector containing information about the vehicle's dynamic state. May + * have different sizes for different observation models. + * + * @return Eigen::VectorXd The vector of expected observations which can have different sizes + * depending on the observation model. + */ + virtual Eigen::VectorXd expected_observations(const Eigen::VectorXd& state) const = 0; + + /** + * @brief Calculates the Jacobian of the expected observations with respect to the state. + * + * @param state The state vector containing information about the vehicle's dynamic state. + * + * @return Eigen::MatrixXd The Jacobian matrix of expected observations with respect to the state. + */ + virtual Eigen::MatrixXd expected_observations_jacobian(const Eigen::VectorXd& state) const = 0; +}; \ No newline at end of file diff --git a/src/perception_sensor_lib/src/observation_model/base_observation_model.cpp b/src/perception_sensor_lib/src/observation_model/slam/slam_base_observation_model.cpp similarity index 87% rename from src/perception_sensor_lib/src/observation_model/base_observation_model.cpp rename to src/perception_sensor_lib/src/observation_model/slam/slam_base_observation_model.cpp index 06ea7df8a..8b133eb6d 100644 --- a/src/perception_sensor_lib/src/observation_model/base_observation_model.cpp +++ b/src/perception_sensor_lib/src/observation_model/slam/slam_base_observation_model.cpp @@ -1,8 +1,8 @@ -#include "perception_sensor_lib/observation_model/base_observation_model.hpp" +#include "perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp" #include "common_lib/maths/transformations.hpp" -Eigen::VectorXd ObservationModel::observation_model( +Eigen::VectorXd SLAMObservationModel::observation_model( const Eigen::VectorXd& state, const std::vector matched_landmarks) const { Eigen::VectorXd matched_landmarks_coordinates(matched_landmarks.size() * 2); for (int i = 0; i < static_cast(matched_landmarks.size()); i++) { @@ -13,12 +13,12 @@ Eigen::VectorXd ObservationModel::observation_model( matched_landmarks_coordinates); } -Eigen::VectorXd ObservationModel::inverse_observation_model( +Eigen::VectorXd SLAMObservationModel::inverse_observation_model( const Eigen::VectorXd& state, const Eigen::VectorXd& observations) const { return common_lib::maths::local_to_global_coordinates(state.segment(0, 3), observations); } -Eigen::MatrixXd ObservationModel::observation_model_jacobian( +Eigen::MatrixXd SLAMObservationModel::observation_model_jacobian( const Eigen::VectorXd& state, const std::vector& matched_landmarks) const { Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(matched_landmarks.size() * 2, state.size()); double car_x = state(0); @@ -44,7 +44,7 @@ Eigen::MatrixXd ObservationModel::observation_model_jacobian( return jacobian; } -Eigen::MatrixXd ObservationModel::inverse_observation_model_jacobian_pose( +Eigen::MatrixXd SLAMObservationModel::inverse_observation_model_jacobian_pose( const Eigen::VectorXd& state, const Eigen::VectorXd& new_landmarks) const { int num_landmarks = new_landmarks.size() / 2; Eigen::MatrixXd gv = Eigen::MatrixXd::Zero(num_landmarks * 2, 3); @@ -60,7 +60,7 @@ Eigen::MatrixXd ObservationModel::inverse_observation_model_jacobian_pose( return gv; } -Eigen::MatrixXd ObservationModel::inverse_observation_model_jacobian_landmarks( +Eigen::MatrixXd SLAMObservationModel::inverse_observation_model_jacobian_landmarks( const Eigen::VectorXd& state, const Eigen::VectorXd& new_landmarks) const { int num_landmarks = new_landmarks.size() / 2; Eigen::MatrixXd gz = Eigen::MatrixXd::Zero(num_landmarks * 2, num_landmarks * 2); diff --git a/src/perception_sensor_lib/src/observation_model/ve/no_slip_bicycle_model.cpp b/src/perception_sensor_lib/src/observation_model/ve/no_slip_bicycle_model.cpp new file mode 100644 index 000000000..d79597844 --- /dev/null +++ b/src/perception_sensor_lib/src/observation_model/ve/no_slip_bicycle_model.cpp @@ -0,0 +1,100 @@ +#include "perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp" + +Eigen::VectorXd NoSlipBicycleModel::expected_observations( + const Eigen::VectorXd& cg_velocities) const { + const double vx = cg_velocities(0); + const double vy = cg_velocities(1); + const double omega = cg_velocities(2); + const double lr = + this->car_parameters_ + ->dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels + const double lf = + this->car_parameters_->wheelbase - + this->car_parameters_ + ->dist_cg_2_rear_axis; // distance from the center of mass to the front wheels + + double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); + double rear_wheels_rpm = + 60 * rear_wheel_velocity / (M_PI * this->car_parameters_->wheel_diameter); + if (vx < 0) { + rear_wheels_rpm = -rear_wheels_rpm; + } + + double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); + double front_wheels_rpm = + 60 * front_wheel_velocity / (M_PI * this->car_parameters_->wheel_diameter); + if (vx < 0) { + front_wheels_rpm = -front_wheels_rpm; + } + double v = sqrt(pow(vx, 2) + pow(vy, 2)); + double steering_angle = + (std::fabs(v) <= 1e-2) ? 0 : atan(omega * (this->car_parameters_->wheelbase) / v); + double motor_rpm = 60 * this->car_parameters_->gear_ratio * rear_wheel_velocity / + (M_PI * this->car_parameters_->wheel_diameter); + if (vx < 0) { + motor_rpm = -motor_rpm; + } + + Eigen::VectorXd observations = Eigen::VectorXd::Zero(6); + observations << front_wheels_rpm, front_wheels_rpm, rear_wheels_rpm, rear_wheels_rpm, + steering_angle, motor_rpm; + return observations; +} + +Eigen::MatrixXd NoSlipBicycleModel::expected_observations_jacobian( + const Eigen::VectorXd& cg_velocities) const { + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 3); + double vx = cg_velocities(0); + double vy = cg_velocities(1); + double omega = cg_velocities(2); + double lr = this->car_parameters_ + ->dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels + double lf = this->car_parameters_->wheelbase - + this->car_parameters_ + ->dist_cg_2_rear_axis; // distance from the center of mass to the front wheels + double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); + double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); + + const double epsilon = 1e-2; + if (std::fabs(rear_wheel_velocity) < epsilon || std::fabs(front_wheel_velocity) < epsilon) { + jacobian(5, 0) = epsilon; + return jacobian; + } + + double sign = (vx < 0) ? -1.0 : 1.0; + jacobian(0, 0) = std::max( + 0.0, sign * 60 * vx / (M_PI * this->car_parameters_->wheel_diameter * front_wheel_velocity)); + jacobian(0, 1) = sign * 60 * (vy + omega * lf) / + (M_PI * this->car_parameters_->wheel_diameter * front_wheel_velocity); + jacobian(0, 2) = sign * 60 * lf * (vy + omega * lf) / + (M_PI * this->car_parameters_->wheel_diameter * front_wheel_velocity); + jacobian(1, 0) = jacobian(0, 0); + jacobian(1, 1) = jacobian(0, 1); + jacobian(1, 2) = jacobian(0, 2); + + jacobian(2, 0) = + sign * 60 * vx / (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); + jacobian(2, 1) = sign * 60 * (vy - omega * lr) / + (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); + jacobian(2, 2) = sign * 60 * (-lr) * (vy - omega * lr) / + (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); + jacobian(3, 0) = jacobian(2, 0); + jacobian(3, 1) = jacobian(2, 1); + jacobian(3, 2) = jacobian(2, 2); + + jacobian(5, 0) = sign * this->car_parameters_->gear_ratio * 60 * vx / + (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); + jacobian(5, 1) = sign * this->car_parameters_->gear_ratio * 60 * (vy - omega * lr) / + (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); + jacobian(5, 2) = sign * this->car_parameters_->gear_ratio * 60 * (-lr) * (vy - omega * lr) / + (M_PI * this->car_parameters_->wheel_diameter * rear_wheel_velocity); + double L = this->car_parameters_->wheelbase; + double v = sqrt(pow(vx, 2) + pow(vy, 2)); + if (std::fabs(v) > epsilon) { + jacobian(4, 0) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vx / v); + jacobian(4, 1) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vy / v); + jacobian(4, 2) = (L * v) / (v * v + (omega * L) * (omega * L)); + } + + return jacobian; +} \ No newline at end of file diff --git a/src/perception_sensor_lib/test/observation_model/base_observation_model_test.cpp b/src/perception_sensor_lib/test/observation_model/slam/slam_base_observation_model_test.cpp similarity index 74% rename from src/perception_sensor_lib/test/observation_model/base_observation_model_test.cpp rename to src/perception_sensor_lib/test/observation_model/slam/slam_base_observation_model_test.cpp index c105bd3e9..f282d511d 100644 --- a/src/perception_sensor_lib/test/observation_model/base_observation_model_test.cpp +++ b/src/perception_sensor_lib/test/observation_model/slam/slam_base_observation_model_test.cpp @@ -1,17 +1,17 @@ -#include "perception_sensor_lib/observation_model/base_observation_model.hpp" - #include #include +#include "perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp" + /** * @brief Test the observation model with trivial state * */ -TEST(ObservationModelTest, test_observation_model_1) { +TEST(SLAMObservationModelTest, test_observation_model_1) { Eigen::VectorXd state(3); state << 0, 0, 0; - ObservationModel observation_model_; + SLAMObservationModel observation_model_; Eigen::VectorXd observations = observation_model_.observation_model(state, std::vector()); EXPECT_FLOAT_EQ(observations.size(), 0); } @@ -20,10 +20,10 @@ TEST(ObservationModelTest, test_observation_model_1) { * @brief Test observation model with a single landmark and trivial pose * */ -TEST(ObservationModelTest, test_observation_model_2) { +TEST(SLAMObservationModelTest, test_observation_model_2) { Eigen::VectorXd state(5); state << 0, 0, 0, 1, 1; - ObservationModel observation_model_; + SLAMObservationModel observation_model_; Eigen::VectorXd observations = observation_model_.observation_model(state, std::vector({3})); EXPECT_FLOAT_EQ(observations.size(), 2); EXPECT_FLOAT_EQ(observations(0), 1); @@ -34,10 +34,10 @@ TEST(ObservationModelTest, test_observation_model_2) { * @brief Test observation model with a single landmark and non-trivial pose * */ -TEST(ObservationModelTest, test_observation_model_3) { +TEST(SLAMObservationModelTest, test_observation_model_3) { Eigen::VectorXd state(5); state << 0, 0, M_PI / 2.0, 1, 1; - ObservationModel observation_model_; + SLAMObservationModel observation_model_; Eigen::VectorXd observations = observation_model_.observation_model(state, std::vector({3})); EXPECT_FLOAT_EQ(observations.size(), 2); EXPECT_FLOAT_EQ(observations(0), 1); @@ -48,10 +48,10 @@ TEST(ObservationModelTest, test_observation_model_3) { * @brief Test inverse observation model with trivial state and a single observation * */ -TEST(ObservationModelTest, test_inverse_observation_model1) { +TEST(SLAMObservationModelTest, test_inverse_observation_model1) { Eigen::VectorXd state(3); state << 0, 0, 0; - ObservationModel observation_model_; + SLAMObservationModel observation_model_; Eigen::VectorXd observations(2); observations << 0, 0; Eigen::VectorXd inverse_observations = diff --git a/src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp b/src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp new file mode 100644 index 000000000..97a8d32b3 --- /dev/null +++ b/src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp @@ -0,0 +1,202 @@ +#include "perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp" + +#include + +#include + +/* ----------------------- ODOMETRY MODEL -------------------------*/ + +/** + * @brief Test a regular case of the conversion from wheel velocities to cg velocities + * + */ +TEST(NoSlipBicycleModelTest, TestCgVelocityToWheels) { + common_lib::car_parameters::CarParameters car_parameters_; + car_parameters_.dist_cg_2_rear_axis = 1.0; + car_parameters_.wheelbase = 2.5; + car_parameters_.wheel_diameter = 0.6; + car_parameters_.gear_ratio = 4.0; + NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); // Example velocities + Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + + ASSERT_EQ(observations.size(), 6); + + EXPECT_NEAR(observations(0), 325.584, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(1), 325.584, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(2), 324.004, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(3), 324.004, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(4), 0.02450, 0.001); // steering_angle + EXPECT_NEAR(observations(5), 1296.02, 0.01); // motor_rpm +} + +/** + * @brief Test the conversion from cg velocities to wheel velocities + * when the velocity in x is negative (the car is moving backwards) + * + */ +TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsNegativeVx) { + common_lib::car_parameters::CarParameters car_parameters_; + car_parameters_.dist_cg_2_rear_axis = 1.0; + car_parameters_.wheelbase = 2.5; + car_parameters_.wheel_diameter = 0.6; + car_parameters_.gear_ratio = 4.0; + NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(-10.0, 2.0, 0.1); // Example velocities + Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + + // Check the size of the observations vector + ASSERT_EQ(observations.size(), 6); + + EXPECT_NEAR(observations(0), -325.584, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(1), -325.584, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(2), -324.004, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(3), -324.004, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(4), 0.02450, 0.001); // steering_angle + EXPECT_NEAR(observations(5), -1296.02, 0.01); // motor_rpm +} + +/** + * @brief Test the conversion from cg velocities to wheel velocities + * when the velocity in x is null and the other velocities are negative + * + */ +TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVx) { + common_lib::car_parameters::CarParameters car_parameters_; + car_parameters_.dist_cg_2_rear_axis = 1.0; + car_parameters_.wheelbase = 2.5; + car_parameters_.wheel_diameter = 0.6; + car_parameters_.gear_ratio = 4.0; + NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(0.0, -2.0, -0.1); // Example velocities + Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + + // Check the size of the observations vector + ASSERT_EQ(observations.size(), 6); + + EXPECT_NEAR(observations(0), 68.4366, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(1), 68.4366, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(2), 60.47888, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(3), 60.47888, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(4), -0.1243, 0.001); // steering_angle + EXPECT_NEAR(observations(5), 241.91552, 0.01); // motor_rpm +} + +/** + * @brief Test the conversion from cg velocities to wheel velocities + * when the velocities are null + * + */ +TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVy) { + common_lib::car_parameters::CarParameters car_parameters_; + car_parameters_.dist_cg_2_rear_axis = 1.0; + car_parameters_.wheelbase = 2.5; + car_parameters_.wheel_diameter = 0.6; + car_parameters_.gear_ratio = 4.0; + NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(0.0, 0.0, 0.0); // Example velocities + Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities); + + // Check the size of the observations vector + ASSERT_EQ(observations.size(), 6); + + EXPECT_NEAR(observations(0), 0, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(1), 0, 0.01); // front_wheels_rpm + EXPECT_NEAR(observations(2), 0, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(3), 0, 0.01); // rear_wheels_rpm + EXPECT_NEAR(observations(4), 0, 0.001); // steering_angle + EXPECT_NEAR(observations(5), 0, 0.01); // motor_rpm +} + +/** + * @brief Test the jacobian of the conversion from cg velocities to wheel velocities + * with regular values of the velocities + * + */ +TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheels) { + // Initialize car parameters + common_lib::car_parameters::CarParameters car_parameters; + car_parameters.dist_cg_2_rear_axis = 1.0; + car_parameters.wheelbase = 2.5; + car_parameters.wheel_diameter = 0.6; + car_parameters.gear_ratio = 4.0; + + // Initialize NoSlipBicycleModel with car parameters + NoSlipBicycleModel bicycle_model(car_parameters); + + // Example velocities + Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); + + // Calculate the Jacobian + Eigen::MatrixXd jacobian = bicycle_model.expected_observations_jacobian(cg_velocities); + + // Check the size of the Jacobian matrix + ASSERT_EQ(jacobian.rows(), 6); + ASSERT_EQ(jacobian.cols(), 3); + + EXPECT_NEAR(jacobian(0, 0), 31.1199, 0.01); + EXPECT_NEAR(jacobian(0, 1), 6.69077, 0.01); + EXPECT_NEAR(jacobian(0, 2), 10.0362, 0.01); + EXPECT_NEAR(jacobian(1, 0), 31.1199, 0.01); + EXPECT_NEAR(jacobian(1, 1), 6.69077, 0.01); + EXPECT_NEAR(jacobian(1, 2), 10.0362, 0.01); + EXPECT_NEAR(jacobian(2, 0), 31.2715, 0.01); + EXPECT_NEAR(jacobian(2, 1), 5.94159, 0.01); + EXPECT_NEAR(jacobian(2, 2), -5.9416, 0.01); + EXPECT_NEAR(jacobian(3, 0), 31.2715, 0.01); + EXPECT_NEAR(jacobian(3, 1), 5.94159, 0.01); + EXPECT_NEAR(jacobian(3, 2), -5.9416, 0.01); + EXPECT_NEAR(jacobian(4, 0), -0.002355, 0.01); + EXPECT_NEAR(jacobian(4, 1), -0.00047, 0.01); + EXPECT_NEAR(jacobian(4, 2), 0.24499, 0.01); + EXPECT_NEAR(jacobian(5, 0), 125.086, 0.01); + EXPECT_NEAR(jacobian(5, 1), 23.7664, 0.01); + EXPECT_NEAR(jacobian(5, 2), -23.7664, 0.01); +} + +/** + * @brief Test the jacobian of the conversion from cg velocities to wheel velocities + * when the velocity in x is negative (the car is moving backwards), the velocity in y is null + * and the angular velocity is negative + * + */ +TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheelsZeroVy) { + // Initialize car parameters + common_lib::car_parameters::CarParameters car_parameters; + car_parameters.dist_cg_2_rear_axis = 1.0; + car_parameters.wheelbase = 2.5; + car_parameters.wheel_diameter = 0.6; + car_parameters.gear_ratio = 4.0; + + // Initialize NoSlipBicycleModel with car parameters + NoSlipBicycleModel bicycle_model(car_parameters); + + // Example velocities + Eigen::Vector3d cg_velocities(-10.0, 0, -0.1); + + // Calculate the Jacobian + Eigen::MatrixXd jacobian = bicycle_model.expected_observations_jacobian(cg_velocities); + + // Check the size of the Jacobian matrix + ASSERT_EQ(jacobian.rows(), 6); + ASSERT_EQ(jacobian.cols(), 3); + + EXPECT_NEAR(jacobian(0, 0), 31.8274082, 0.01); + EXPECT_NEAR(jacobian(0, 1), 0.47741112, 0.01); + EXPECT_NEAR(jacobian(0, 2), 0.71611668, 0.01); + EXPECT_NEAR(jacobian(1, 0), 31.8274082, 0.01); + EXPECT_NEAR(jacobian(1, 1), 0.47741112, 0.01); + EXPECT_NEAR(jacobian(1, 2), 0.71611668, 0.01); + EXPECT_NEAR(jacobian(2, 0), 31.8293971, 0.01); + EXPECT_NEAR(jacobian(2, 1), -0.3182939, 0.01); + EXPECT_NEAR(jacobian(2, 2), 0.31829397, 0.01); + EXPECT_NEAR(jacobian(3, 0), 31.8293971, 0.01); + EXPECT_NEAR(jacobian(3, 1), -0.3182939, 0.01); + EXPECT_NEAR(jacobian(3, 2), 0.31829397, 0.01); + EXPECT_NEAR(jacobian(4, 0), -0.00149966, 0.01); + EXPECT_NEAR(jacobian(4, 1), 0.0, 0.01); + EXPECT_NEAR(jacobian(4, 2), 0.24984, 0.01); + EXPECT_NEAR(jacobian(5, 0), 127.3175884, 0.01); + EXPECT_NEAR(jacobian(5, 1), -1.27316, 0.01); + EXPECT_NEAR(jacobian(5, 2), 1.27316, 0.01); +} \ No newline at end of file diff --git a/src/slam/include/slam_solver/ekf_slam_solver.hpp b/src/slam/include/slam_solver/ekf_slam_solver.hpp index 083590f28..86cb5a475 100644 --- a/src/slam/include/slam_solver/ekf_slam_solver.hpp +++ b/src/slam/include/slam_solver/ekf_slam_solver.hpp @@ -5,13 +5,13 @@ #include "common_lib/conversions/cones.hpp" #include "common_lib/maths/transformations.hpp" -#include "perception_sensor_lib/observation_model/base_observation_model.hpp" +#include "perception_sensor_lib/observation_model/slam/slam_base_observation_model.hpp" #include "slam_solver/slam_solver.hpp" #include "solver_traits/velocities_integrator_trait.hpp" class EKFSLAMSolver : public SLAMSolver, public VelocitiesIntegratorTrait { SLAMParameters slam_parameters_; - std::shared_ptr observation_model_; + std::shared_ptr observation_model_; Eigen::VectorXd state_ = Eigen::VectorXd::Zero(3); Eigen::MatrixXd covariance_; Eigen::MatrixXd process_noise_matrix_; diff --git a/src/slam/src/slam_solver/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp index 722863b7d..dec7dd463 100644 --- a/src/slam/src/slam_solver/ekf_slam_solver.cpp +++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp @@ -17,7 +17,7 @@ EKFSLAMSolver::EKFSLAMSolver(const SLAMParameters& params, this->process_noise_matrix_(0, 0) = params.velocity_x_noise_; this->process_noise_matrix_(1, 1) = params.velocity_y_noise_; this->process_noise_matrix_(2, 2) = params.angular_velocity_noise_; - this->observation_model_ = std::make_shared(); + this->observation_model_ = std::make_shared(); } Eigen::MatrixXd EKFSLAMSolver::get_observation_noise_matrix(int num_landmarks) const { diff --git a/src/velocity_estimation/CMakeLists.txt b/src/velocity_estimation/CMakeLists.txt index 494a8d4f7..275321d8c 100644 --- a/src/velocity_estimation/CMakeLists.txt +++ b/src/velocity_estimation/CMakeLists.txt @@ -5,6 +5,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -O2) endif() +# Find PCL +find_package(PCL REQUIRED COMPONENTS registration common io) + # find dependencies set(PACKAGES ament_cmake diff --git a/src/velocity_estimation/include/estimators/ekf.hpp b/src/velocity_estimation/include/estimators/ekf.hpp index bee548001..e982b5553 100644 --- a/src/velocity_estimation/include/estimators/ekf.hpp +++ b/src/velocity_estimation/include/estimators/ekf.hpp @@ -12,7 +12,7 @@ #include "custom_interfaces/msg/velocities.hpp" #include "estimators/estimator.hpp" #include "motion_lib/vel_process_model/map.hpp" -#include "perception_sensor_lib/observation_model/ve_observation_model/map.hpp" +#include "perception_sensor_lib/observation_model/ve/map.hpp" class EKF : public VelocityEstimator { rclcpp::Time _last_update_; diff --git a/src/velocity_estimation/include/estimators/no_rear_wss_ekf.hpp b/src/velocity_estimation/include/estimators/no_rear_wss_ekf.hpp index 9a1ce0c0f..7e6a8fa46 100644 --- a/src/velocity_estimation/include/estimators/no_rear_wss_ekf.hpp +++ b/src/velocity_estimation/include/estimators/no_rear_wss_ekf.hpp @@ -12,7 +12,7 @@ #include "custom_interfaces/msg/velocities.hpp" #include "estimators/estimator.hpp" #include "motion_lib/vel_process_model/map.hpp" -#include "perception_sensor_lib/observation_model/ve_observation_model/map.hpp" +#include "perception_sensor_lib/observation_model/ve/map.hpp" class NoRearWSSEKF : public VelocityEstimator { rclcpp::Time _last_update_; From 9463f9d1eddbc1059f9d088d523c85994f796770 Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Tue, 11 Nov 2025 12:43:25 +0000 Subject: [PATCH 03/10] motion lib problems --- config/slam/vehicle.yaml | 8 +- src/motion_lib/CMakeLists.txt | 9 +- .../motion_lib/aero_model/base_aero_model.hpp | 22 +++ .../aero_model/default_aero_model.hpp | 8 ++ .../include/motion_lib/aero_model/map.hpp | 22 +++ .../base_load_transfer_model.hpp | 30 ++++ .../motion_lib/load_transfer_model/map.hpp | 23 +++ .../load_transfer_model/rigid_body_model.hpp | 16 +++ .../steering_model/base_steering_model.hpp | 23 +++ .../include/motion_lib/steering_model/map.hpp | 23 +++ .../parallel_front_steering.hpp | 14 ++ .../base_steering_motor_model.hpp | 25 ++++ .../motion_lib/steering_motor_model/map.hpp | 24 ++++ .../pid_steering_motor.hpp | 25 ++++ .../motion_lib/tire_model/base_tire_model.hpp | 27 ++++ .../include/motion_lib/tire_model/map.hpp | 22 +++ .../tire_model/pacejka_combined_slip.hpp | 9 ++ .../src/aero_model/default_aero_model.cpp | 24 ++++ .../load_transfer_model/rigid_body_model.cpp | 41 ++++++ .../src/s2v_model/bicycle_model.cpp | 135 ------------------ .../s2v_model/no_rear_wss_bicycle_model.cpp | 120 ---------------- .../src/s2v_model/no_wss_bicycle_model.cpp | 73 ---------- .../parallel_front_steering.cpp | 6 + .../pid_steering_motor.cpp | 24 ++++ .../src/tire_model/pacejka_combined_slip.cpp | 32 +++++ 25 files changed, 449 insertions(+), 336 deletions(-) create mode 100644 src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp create mode 100644 src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp create mode 100644 src/motion_lib/include/motion_lib/aero_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp create mode 100644 src/motion_lib/include/motion_lib/load_transfer_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp create mode 100644 src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp create mode 100644 src/motion_lib/include/motion_lib/steering_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp create mode 100644 src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp create mode 100644 src/motion_lib/include/motion_lib/steering_motor_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp create mode 100644 src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp create mode 100644 src/motion_lib/include/motion_lib/tire_model/map.hpp create mode 100644 src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp create mode 100644 src/motion_lib/src/aero_model/default_aero_model.cpp create mode 100644 src/motion_lib/src/load_transfer_model/rigid_body_model.cpp delete mode 100644 src/motion_lib/src/s2v_model/bicycle_model.cpp delete mode 100644 src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp delete mode 100644 src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp create mode 100644 src/motion_lib/src/steering_model/parallel_front_steering.cpp create mode 100644 src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp create mode 100644 src/motion_lib/src/tire_model/pacejka_combined_slip.cpp diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index 959ea6ce1..ab5ba4098 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -6,7 +6,7 @@ slam: receive_lidar_odometry: false # Whether to use lidar odometry topic or not data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model data_association_limit_distance: 70 # maximum distance to consider a cone for data association - data_association_gate: 0.9 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark + data_association_gate: 1.5 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark new_landmark_confidence_gate: 0.6 # Minimum confidence to consider an observation as a new landmark in data asociation observation_x_noise: 0.02 # sigma of the noise for cone positions in x (range in Graph SLAM) observation_y_noise: 0.001 # sigma of the noise for cone positions in y (bearing in Graph SLAM) @@ -22,13 +22,13 @@ slam: slam_solver_name: "graph_slam" # "graph_slam" or "ekf_slam" or ... -> name of the slam solver slam_optimization_mode: "async" # "sync" or "sync-parallel" or "async" or ... -> name of the optimization mode, in which sync is at every factor added, async is at every time step and sync-parallel is sync but callbacks execute in parallel slam_optimization_in_poses: false # If true, the optimization will be done also when poses are added, not only when observations are added - slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type + slam_optimization_type: "isam2" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type # Only for pose_updater_name: "difference_based_ready" slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph # Only for slam_optimization_mode: "async" - slam_optimization_period: 0.05 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added + slam_optimization_period: 0.2 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added # Only for slam_optimization_type: "isam2" slam_isam2_relinearize_threshold: 0.1 # Threshold for relinearizing the graph in isam2 @@ -45,5 +45,5 @@ slam: # Loop closure paramteters threshold_dist: 4.0 # Distance around origin to trigger loop closure first_x_cones: 10 # consider a loop if you see any of these first X cones - border_width: 5 # distance to givve to start searching for loop closure again + border_width: 5 # distance to give to start searching for loop closure again minimum_confidence: 3 # minimum number of observations to confirm loop closure \ No newline at end of file diff --git a/src/motion_lib/CMakeLists.txt b/src/motion_lib/CMakeLists.txt index 8222e212f..0487a61f4 100644 --- a/src/motion_lib/CMakeLists.txt +++ b/src/motion_lib/CMakeLists.txt @@ -27,12 +27,14 @@ set(IMPLEMENTATION_FILES src/vel_process_model/particle_model.cpp src/v2p_models/constant_velocity_model.cpp src/v2p_models/base_v2p_motion_model.cpp - src/s2v_model/bicycle_model.cpp - src/s2v_model/no_rear_wss_bicycle_model.cpp - src/s2v_model/no_wss_bicycle_model.cpp src/v2p_models/constant_velocity_turnrate_model.cpp src/v2p_models/constant_acceleration_turnrate_model.cpp src/v2p_models/odometry_model.cpp + src/aero_model/default_aero_model.cpp + src/tire_model/pacejka_combined_slip.cpp + src/steering_model/parallel_front_steering.cpp + src/load_transfer_model/rigid_body_model.cpp + src/steering_motor_model/pid_steering_motor.cpp ) add_library(${PROJECT_NAME} SHARED ${IMPLEMENTATION_FILES}) @@ -65,7 +67,6 @@ if(BUILD_TESTING) set(TESTFILES test/main.cpp - test/s2v_models/bicycle_model_test.cpp test/v2p_models/constant_velocity_model_test.cpp test/v2p_models/constant_velocity_turnrate_model_test.cpp test/v2p_models/constant_acceleration_turnrate_model_test.cpp diff --git a/src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp b/src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp new file mode 100644 index 000000000..587947d10 --- /dev/null +++ b/src/motion_lib/include/motion_lib/aero_model/base_aero_model.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "common_lib/car_parameters/car_parameters.hpp" + +class AeroModel { +protected: + std::shared_ptr car_parameters_; + +public: + AeroModel(const common_lib::car_parameters::CarParameters& car_parameters) + : car_parameters_( + std::make_shared(car_parameters)) {} + /** + * @brief Calculate the aero forces + * + * @param velocity Velocity of the car in m/s in the car's body frame [vx, vy, angular_velocity] + * @return Eigen::Vector3d Aero forces in the car's body frame [Fx, Fy, Fz] in Newtons + */ + virtual Eigen::Vector3d aero_forces(const Eigen::Vector3d& velocity) const = 0; +}; diff --git a/src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp b/src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp new file mode 100644 index 000000000..7c6e64e5a --- /dev/null +++ b/src/motion_lib/include/motion_lib/aero_model/default_aero_model.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include "base_aero_model.hpp" + +class DefaultAeroModel : public AeroModel { +public: + Eigen::Vector3d aero_forces(const Eigen::Vector3d& velocity) const override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/aero_model/map.hpp b/src/motion_lib/include/motion_lib/aero_model/map.hpp new file mode 100644 index 000000000..455b048f0 --- /dev/null +++ b/src/motion_lib/include/motion_lib/aero_model/map.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include +#include +#include + +#include "default_aero_model.hpp" + +/* + * Map of aero models, with the key being the name of the aero model and the value being a lambda + * function that returns a shared pointer to the corresponding aero model + */ +const std::map< + std::string, + std::function(const common_lib::car_parameters::CarParameters&)>, + std::less<>> + aero_models_map = { + {"default_aero", + [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp b/src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp new file mode 100644 index 000000000..209e859a7 --- /dev/null +++ b/src/motion_lib/include/motion_lib/load_transfer_model/base_load_transfer_model.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +#include "common_lib/car_parameters/car_parameters.hpp" + +/** + * @brief Base class for models that compute load transfer on a vehicle. + * + */ +class LoadTransferModel { +protected: + std::shared_ptr car_parameters_; + +public: + LoadTransferModel(const common_lib::car_parameters::CarParameters& car_parameters) + : car_parameters_( + std::make_shared(car_parameters)) {} + + /** + * @brief Computes loads on the tires based on the dynamic state of the vehicle. + * + * @param dynamic_state Contains the relevant dynamic state that affects the load transfer, can + * include accelerations, euler angles, velocities, etc. + * @return Eigen::Vector4d a vector containing the loads on the four tires in Newtons, in the + * order: FL, FR, RL, RR. + */ + virtual Eigen::Vector4d compute_loads(const Eigen::VectorXd& dynamic_state) const = 0; +}; diff --git a/src/motion_lib/include/motion_lib/load_transfer_model/map.hpp b/src/motion_lib/include/motion_lib/load_transfer_model/map.hpp new file mode 100644 index 000000000..437f458b2 --- /dev/null +++ b/src/motion_lib/include/motion_lib/load_transfer_model/map.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include +#include + +#include "motion_lib/load_transfer_model/rigid_body_model.hpp" + +/* + * Map of load transfer models, with the key being the name of the model and the value being a + * lambda function that returns a shared pointer to the corresponding load transfer model. + */ +const std::map( + const common_lib::car_parameters::CarParameters&)>, + std::less<>> + load_transfer_models_map = { + {"rigid_body", + [](const common_lib::car_parameters::CarParameters& params) + -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp b/src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp new file mode 100644 index 000000000..53f87430b --- /dev/null +++ b/src/motion_lib/include/motion_lib/load_transfer_model/rigid_body_model.hpp @@ -0,0 +1,16 @@ +#include "motion_lib/load_transfer_model/base_load_transfer_model.hpp" + +class RigidBodyLoadTransferModel : public LoadTransferModel { +public: + RigidBodyLoadTransferModel(const common_lib::car_parameters::CarParameters& car_parameters) + : LoadTransferModel(car_parameters) {} + + /** + * @brief Computes loads on the tires based on the dynamic state of the vehicle. + * + * @param dynamic_state Receives only the lateral and longitudinal accelerations + * @return Eigen::Vector4d a vector containing the loads on the four tires in Newtons, in the + * order: FL, FR, RL, RR. + */ + Eigen::Vector4d compute_loads(const Eigen::VectorXd& dynamic_state) const override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp b/src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp new file mode 100644 index 000000000..d846bc1aa --- /dev/null +++ b/src/motion_lib/include/motion_lib/steering_model/base_steering_model.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include "common_lib/car_parameters/car_parameters.hpp" + +class SteeringModel { +protected: + std::shared_ptr car_parameters_; + +public: + SteeringModel(const common_lib::car_parameters::CarParameters& car_parameters) + : car_parameters_( + std::make_shared(car_parameters)) {} + /** + * @brief Calculate the steering angles on each wheel based on the steering wheel angle. + * + * @param steering_wheel_angle The angle of the steering wheel in radians. + * @return Eigen::Vector4d A vector containing the steering angles for each wheel: + * [front left, front right, rear left, rear right] in radians. + */ + virtual Eigen::Vector4d calculate_steering_angles(double steering_wheel_angle) const = 0; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_model/map.hpp b/src/motion_lib/include/motion_lib/steering_model/map.hpp new file mode 100644 index 000000000..cbd98b0c3 --- /dev/null +++ b/src/motion_lib/include/motion_lib/steering_model/map.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include +#include + +#include "parallel_front_steering.hpp" + +/* + * Map of steering models, with the key being the name of the steering model and the value being a + * lambda function that returns a shared pointer to the corresponding steering model + */ +const std::map< + std::string, + std::function(const common_lib::car_parameters::CarParameters&)>, + std::less<>> + steering_models_map = { + {"parallel_front_steering", + [](const common_lib::car_parameters::CarParameters& params) + -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp b/src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp new file mode 100644 index 000000000..86e995ab1 --- /dev/null +++ b/src/motion_lib/include/motion_lib/steering_model/parallel_front_steering.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "motion_lib/steering_model/base_steering_model.hpp" +/** + * @brief Steering model that assumes parallel front steering, meaning that front wheels turn as + * much as the steering wheel angle, and rear wheels do not steer. + * + */ +class ParallelFrontSteering : public SteeringModel { +public: + explicit ParallelFrontSteering(common_lib::car_parameters::CarParameters car_parameters) + : SteeringModel(car_parameters) {} + Eigen::Vector4d calculate_steering_angles(double steering_wheel_angle) const override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp b/src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp new file mode 100644 index 000000000..ebe377c4d --- /dev/null +++ b/src/motion_lib/include/motion_lib/steering_motor_model/base_steering_motor_model.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +#include "common_lib/car_parameters/car_parameters.hpp" + +class SteeringMotorModel { +protected: + std::shared_ptr car_parameters_; + +public: + SteeringMotorModel(const common_lib::car_parameters::CarParameters& car_parameters) + : car_parameters_( + std::make_shared(car_parameters)) {} + + /** + * @brief Computes the change in steering angle based. + * + * @param current_steering current steering angle in radians + * @param steering_goal desired steering angle in radians + * @return double change in steering angle in radians + */ + virtual double compute_steering_rate(double current_steering, double steering_goal) = 0; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/steering_motor_model/map.hpp b/src/motion_lib/include/motion_lib/steering_motor_model/map.hpp new file mode 100644 index 000000000..af7d9cba7 --- /dev/null +++ b/src/motion_lib/include/motion_lib/steering_motor_model/map.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +#include "motion_lib/steering_motor_model/pid_steering_motor.hpp" + +/* + * Map of steering motor models, with the key being the name of the steering motor model and the + * value being a lambda function that returns a shared pointer to the corresponding steering motor + * model + */ +const std::map( + const common_lib::car_parameters::CarParameters&)>, + std::less<>> + steering_motor_models_map = { + {"pid", + [](const common_lib::car_parameters::CarParameters& params) + -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp b/src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp new file mode 100644 index 000000000..ee6a6eceb --- /dev/null +++ b/src/motion_lib/include/motion_lib/steering_motor_model/pid_steering_motor.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include "motion_lib/steering_motor_model/base_steering_motor_model.hpp" + +class PIDSteeringMotor : public SteeringMotorModel { +private: + double integral_ = 0.0; // Integral term accumulator + double previous_error_ = 0.0; // Previous error for derivative calculation + rclcpp::Time last_update = rclcpp::Time(0, 0); + +public: + PIDSteeringMotor(const common_lib::car_parameters::CarParameters& car_parameters) + : SteeringMotorModel(car_parameters) {} + + /** + * @brief Computes the change in steering angle based on PID control. + * + * @param current_steering current steering angle in radians + * @param steering_goal desired steering angle in radians + * @return double change in steering angle in radians + */ + double compute_steering_rate(double current_steering, double steering_goal) override; +}; \ No newline at end of file diff --git a/src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp b/src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp new file mode 100644 index 000000000..cb94d9056 --- /dev/null +++ b/src/motion_lib/include/motion_lib/tire_model/base_tire_model.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "common_lib/car_parameters/car_parameters.hpp" +/** + * @brief Class used to model tires. Currently used to model tire forces based on slip. + * + */ +class TireModel { +protected: + std::shared_ptr car_parameters_; + +public: + TireModel(const common_lib::car_parameters::CarParameters& car_parameters) + : car_parameters_( + std::make_shared(car_parameters)) {} + /** + * @brief Calculate the forces acting in a tire based on the tire characteristics and dynamic + * state. + * + * @param slip_angle Slip angle of the tire in radians + * @param slip_ratio Slip ratio of the tire + * @param load Load on the tire in Newtons + * @return std::pair Longitudinal and lateral forces + */ + virtual std::pair tire_forces(double slip_angle, double slip_ratio, + double vertical_load) const = 0; +}; diff --git a/src/motion_lib/include/motion_lib/tire_model/map.hpp b/src/motion_lib/include/motion_lib/tire_model/map.hpp new file mode 100644 index 000000000..fe64f73c2 --- /dev/null +++ b/src/motion_lib/include/motion_lib/tire_model/map.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include +#include +#include + +#include "pacejka_combined_slip.hpp" + +/* + * Map of tire models, with the key being the name of the tire model and the value being a lambda + * function that returns a shared pointer to the corresponding tire model + */ +const std::map< + std::string, + std::function(const common_lib::car_parameters::CarParameters&)>, + std::less<>> + tire_models_map = { + {"pacejka_combined_slip", + [](const common_lib::car_parameters::CarParameters& params) -> std::shared_ptr { + return std::make_shared(params); + }}, +}; diff --git a/src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp b/src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp new file mode 100644 index 000000000..52c848441 --- /dev/null +++ b/src/motion_lib/include/motion_lib/tire_model/pacejka_combined_slip.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include "base_tire_model.hpp" + +class PacejkaCombinedSlip : public TireModel { +public: + std::pair tire_forces(double slip_angle, double slip_ratio, + double vertical_load) const override; +}; \ No newline at end of file diff --git a/src/motion_lib/src/aero_model/default_aero_model.cpp b/src/motion_lib/src/aero_model/default_aero_model.cpp new file mode 100644 index 000000000..6cd23e21b --- /dev/null +++ b/src/motion_lib/src/aero_model/default_aero_model.cpp @@ -0,0 +1,24 @@ +#include "motion_lib/aero_model/default_aero_model.hpp" + +Eigen::Vector3d DefaultAeroModel::aero_forces(const Eigen::Vector3d& velocity) const { + const double vx = velocity[0]; + const double vy = velocity[1]; + + const double air_density = 1.225; // [kg/m^3] TODO (maybe): make this a parameter + const double frontal_area = this->car_parameters_->aero_parameters.frontal_area; // [m^2] + const double drag_coefficient = this->car_parameters_->aero_parameters.drag_coefficient; + const double side_force_coefficient = + this->car_parameters_->aero_parameters.aero_side_force_coefficient; + const double lift_coefficient = this->car_parameters_->aero_parameters.lift_coefficient; + + // Drag force (opposes vx) + const double Fx = -0.5 * air_density * frontal_area * drag_coefficient * vx * std::abs(vx); + + // Side force (opposes vy) + const double Fy = -0.5 * air_density * frontal_area * side_force_coefficient * vy * std::abs(vy); + + // Downforce/Lift + const double Fz = -0.5 * air_density * frontal_area * lift_coefficient * vx * vx; + + return Eigen::Vector3d(Fx, Fy, Fz); +} \ No newline at end of file diff --git a/src/motion_lib/src/load_transfer_model/rigid_body_model.cpp b/src/motion_lib/src/load_transfer_model/rigid_body_model.cpp new file mode 100644 index 000000000..00b047304 --- /dev/null +++ b/src/motion_lib/src/load_transfer_model/rigid_body_model.cpp @@ -0,0 +1,41 @@ +#include "motion_lib/load_transfer_model/rigid_body_model.hpp" + +Eigen::Vector4d RigidBodyLoadTransferModel::compute_loads( + const Eigen::VectorXd& dynamic_state) const { + if (dynamic_state.size() != 2) { + throw std::invalid_argument( + "Dynamic state for Rigid Body Load Transfer Model must contain exactly two elements: " + "lateral and longitudinal accelerations."); + } + + // Accelerations at the CG + double longitudinal_acceleration = dynamic_state(0); + double lateral_acceleration = dynamic_state(1); + + double mass = this->car_parameters_->mass; + double g = 9.8065; // m/s^2 TODO: make this a constant in the car parameters (maybe?) + double cog_height = this->car_parameters_->cog_height; + double wheelbase = this->car_parameters_->wheelbase; + double front_weight_distribution = + this->car_parameters_->dist_cg_2_rear_axis / this->car_parameters_->wheelbase; + double rear_weight_distribution = 1.0 - front_weight_distribution; + double track_width = this->car_parameters_->track_width; + + // Static load per wheel + double Fz_total = mass * g; + double front_wheel_static_load = front_weight_distribution * Fz_total / 2.0; + double rear_wheel_static_load = rear_weight_distribution * Fz_total / 2.0; + + // Weight transfer for each axle + double longitudinal_transfer = (mass * longitudinal_acceleration * cog_height) / wheelbase; + double lateral_transfer = (mass * lateral_acceleration * cog_height) / track_width; + + Eigen::Vector4d loads; + + loads(0) = front_wheel_static_load - longitudinal_transfer / 2.0 - lateral_transfer / 2.0; + loads(1) = front_wheel_static_load - longitudinal_transfer / 2.0 + lateral_transfer / 2.0; + loads(2) = rear_wheel_static_load + longitudinal_transfer / 2.0 - lateral_transfer / 2.0; + loads(3) = rear_wheel_static_load + longitudinal_transfer / 2.0 + lateral_transfer / 2.0; + + return loads; +} \ No newline at end of file diff --git a/src/motion_lib/src/s2v_model/bicycle_model.cpp b/src/motion_lib/src/s2v_model/bicycle_model.cpp deleted file mode 100644 index 41fd0bbb3..000000000 --- a/src/motion_lib/src/s2v_model/bicycle_model.cpp +++ /dev/null @@ -1,135 +0,0 @@ -#include "motion_lib/s2v_model/bicycle_model.hpp" - -std::pair BicycleModel::wheels_velocities_to_cg(double rl_rpm, - [[maybe_unused]] double fl_rpm, - double rr_rpm, - [[maybe_unused]] double fr_rpm, - double steering_angle) { - std::pair velocities = {0, 0}; - double rl_velocity = - common_lib::maths::get_wheel_velocity_from_rpm(rl_rpm, this->car_parameters_.wheel_diameter); - double rr_velocity = - common_lib::maths::get_wheel_velocity_from_rpm(rr_rpm, this->car_parameters_.wheel_diameter); - if (steering_angle == 0) { // If no steering angle, moving straight - velocities.first = (rl_velocity + rr_velocity) / 2; - } else if (steering_angle > 0) { - double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); - velocities.second = - rl_velocity / (rear_axis_center_rotation_radius - (this->car_parameters_.track_width / 2)); - velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + - pow(this->car_parameters_.rear_axis_to_camera, 2)) * - fabs(velocities.second); - } else { - double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); - velocities.second = - rr_velocity / (rear_axis_center_rotation_radius + (this->car_parameters_.track_width / 2)); - velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + - pow(this->car_parameters_.rear_axis_to_camera, 2)) * - fabs(velocities.second); - } - return velocities; -} - -common_lib::structures::Position BicycleModel::rear_axis_position( - const common_lib::structures::Position& cg, double orientation, double dist_cg_2_rear_axis) { - common_lib::structures::Position rear_axis; - rear_axis.x = cg.x - dist_cg_2_rear_axis * cos(orientation); - rear_axis.y = cg.y - dist_cg_2_rear_axis * sin(orientation); - return rear_axis; -} - -Eigen::VectorXd BicycleModel::cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { - const double vx = cg_velocities(0); - const double vy = cg_velocities(1); - const double omega = cg_velocities(2); - const double lr = - this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels - const double lf = - this->car_parameters_.wheelbase - - this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels - - double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); - double rear_wheels_rpm = 60 * rear_wheel_velocity / (M_PI * this->car_parameters_.wheel_diameter); - if (vx < 0) { - rear_wheels_rpm = -rear_wheels_rpm; - } - - double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); - double front_wheels_rpm = - 60 * front_wheel_velocity / (M_PI * this->car_parameters_.wheel_diameter); - if (vx < 0) { - front_wheels_rpm = -front_wheels_rpm; - } - double v = sqrt(pow(vx, 2) + pow(vy, 2)); - double steering_angle = - (std::fabs(v) <= 0.1) ? 0 : atan(omega * (this->car_parameters_.wheelbase) / v); - double motor_rpm = 60 * this->car_parameters_.gear_ratio * rear_wheel_velocity / - (M_PI * this->car_parameters_.wheel_diameter); - if (vx < 0) { - motor_rpm = -motor_rpm; - } - - Eigen::VectorXd observations = Eigen::VectorXd::Zero(6); - observations << front_wheels_rpm, front_wheels_rpm, rear_wheels_rpm, rear_wheels_rpm, - steering_angle, motor_rpm; - return observations; -} - -Eigen::MatrixXd BicycleModel::jacobian_cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 3); - double vx = cg_velocities(0); - double vy = cg_velocities(1); - double omega = cg_velocities(2); - double lr = this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels - double lf = this->car_parameters_.wheelbase - - this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels - double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); - double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); - - const double epsilon = 1e-3; - if (std::fabs(rear_wheel_velocity) < epsilon || std::fabs(front_wheel_velocity) < epsilon) { - jacobian(0, 0) = epsilon; - return jacobian; - } - - double sign = (vx < 0) ? -1.0 : 1.0; - jacobian(0, 0) = - sign * 60 * vx / (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); - jacobian(0, 1) = sign * 60 * (vy + omega * lf) / - (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); - jacobian(0, 2) = sign * 60 * lf * (vy + omega * lf) / - (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); - jacobian(1, 0) = jacobian(0, 0); - jacobian(1, 1) = jacobian(0, 1); - jacobian(1, 2) = jacobian(0, 2); - - jacobian(2, 0) = - sign * 60 * vx / (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(2, 1) = sign * 60 * (vy - omega * lr) / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(2, 2) = sign * 60 * (-lr) * (vy - omega * lr) / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(3, 0) = jacobian(2, 0); - jacobian(3, 1) = jacobian(2, 1); - jacobian(3, 2) = jacobian(2, 2); - - jacobian(5, 0) = sign * this->car_parameters_.gear_ratio * 60 * vx / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(5, 1) = sign * this->car_parameters_.gear_ratio * 60 * (vy - omega * lr) / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(5, 2) = sign * this->car_parameters_.gear_ratio * 60 * (-lr) * (vy - omega * lr) / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - double L = this->car_parameters_.wheelbase; - double v = sqrt(pow(vx, 2) + pow(vy, 2)); - if (std::fabs(v) > epsilon) { - jacobian(4, 0) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vx / v); - jacobian(4, 1) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vy / v); - jacobian(4, 2) = (L * v) / (v * v + (omega * L) * (omega * L)); - } - - return jacobian; -} \ No newline at end of file diff --git a/src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp b/src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp deleted file mode 100644 index 78b1f1716..000000000 --- a/src/motion_lib/src/s2v_model/no_rear_wss_bicycle_model.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include "motion_lib/s2v_model/no_rear_wss_bicycle_model.hpp" - -std::pair NoRearWSSBicycleModel::wheels_velocities_to_cg( - double rl_rpm, [[maybe_unused]] double fl_rpm, double rr_rpm, [[maybe_unused]] double fr_rpm, - double steering_angle) { - std::pair velocities = {0, 0}; - double rl_velocity = - common_lib::maths::get_wheel_velocity_from_rpm(rl_rpm, this->car_parameters_.wheel_diameter); - double rr_velocity = - common_lib::maths::get_wheel_velocity_from_rpm(rr_rpm, this->car_parameters_.wheel_diameter); - if (steering_angle == 0) { // If no steering angle, moving straight - velocities.first = (rl_velocity + rr_velocity) / 2; - } else if (steering_angle > 0) { - double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); - velocities.second = - rl_velocity / (rear_axis_center_rotation_radius - (this->car_parameters_.track_width / 2)); - velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + - pow(this->car_parameters_.rear_axis_to_camera, 2)) * - fabs(velocities.second); - } else { - double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); - velocities.second = - rr_velocity / (rear_axis_center_rotation_radius + (this->car_parameters_.track_width / 2)); - velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + - pow(this->car_parameters_.rear_axis_to_camera, 2)) * - fabs(velocities.second); - } - return velocities; -} - -common_lib::structures::Position NoRearWSSBicycleModel::rear_axis_position( - const common_lib::structures::Position& cg, double orientation, double dist_cg_2_rear_axis) { - common_lib::structures::Position rear_axis; - rear_axis.x = cg.x - dist_cg_2_rear_axis * cos(orientation); - rear_axis.y = cg.y - dist_cg_2_rear_axis * sin(orientation); - return rear_axis; -} - -Eigen::VectorXd NoRearWSSBicycleModel::cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { - const double vx = cg_velocities(0); - const double vy = cg_velocities(1); - const double omega = cg_velocities(2); - const double lr = - this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels - const double lf = - this->car_parameters_.wheelbase - - this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels - - double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); - - double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); - double front_wheels_rpm = - 60 * front_wheel_velocity / (M_PI * this->car_parameters_.wheel_diameter); - if (vx < 0) { - front_wheels_rpm = -front_wheels_rpm; - } - double v = sqrt(pow(vx, 2) + pow(vy, 2)); - double steering_angle = - (std::fabs(v) <= 1e-2) ? 0 : atan(omega * (this->car_parameters_.wheelbase) / v); - double motor_rpm = 60 * this->car_parameters_.gear_ratio * rear_wheel_velocity / - (M_PI * this->car_parameters_.wheel_diameter); - if (vx < 0) { - motor_rpm = -motor_rpm; - } - - Eigen::VectorXd observations = Eigen::VectorXd::Zero(4); - observations << front_wheels_rpm, front_wheels_rpm, steering_angle, motor_rpm; - return observations; -} - -Eigen::MatrixXd NoRearWSSBicycleModel::jacobian_cg_velocity_to_wheels( - const Eigen::Vector3d& cg_velocities) { - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(4, 3); - double vx = cg_velocities(0); - double vy = cg_velocities(1); - double omega = cg_velocities(2); - double lr = this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the rear wheels - double lf = this->car_parameters_.wheelbase - - this->car_parameters_ - .dist_cg_2_rear_axis; // distance from the center of mass to the front wheels - double rear_wheel_velocity = sqrt(pow(vx, 2) + pow(vy - omega * lr, 2)); - double front_wheel_velocity = sqrt(pow(vx, 2) + pow(vy + omega * lf, 2)); - - const double epsilon = 1e-2; - if (std::fabs(rear_wheel_velocity) < epsilon || std::fabs(front_wheel_velocity) < epsilon) { - jacobian(3, 0) = epsilon; - return jacobian; - } - - double sign = (vx < 0) ? -1.0 : 1.0; - jacobian(0, 0) = std::max( - 0.0, sign * 60 * vx / (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity)); - jacobian(0, 1) = 0; - // std::max(0.0, sign * 60 * (vy + omega * lf) / - // (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity)); - jacobian(0, 2) = sign * 60 * lf * (vy + omega * lf) / - (M_PI * this->car_parameters_.wheel_diameter * front_wheel_velocity); - jacobian(1, 0) = jacobian(0, 0); - jacobian(1, 1) = jacobian(0, 1); - jacobian(1, 2) = jacobian(0, 2); - - jacobian(3, 0) = sign * this->car_parameters_.gear_ratio * 60 * vx / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(3, 1) = 0; // sign * this->car_parameters_.gear_ratio * 60 * (vy - omega * lr) / - //(M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - jacobian(3, 2) = sign * this->car_parameters_.gear_ratio * 60 * (-lr) * (vy - omega * lr) / - (M_PI * this->car_parameters_.wheel_diameter * rear_wheel_velocity); - double L = this->car_parameters_.wheelbase; - double v = sqrt(pow(vx, 2) + pow(vy, 2)); - if (std::fabs(v) > epsilon) { - jacobian(2, 0) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vx / v); - jacobian(2, 1) = -((omega * L) / (v * v + (omega * L) * (omega * L))) * (vy / v); - jacobian(2, 2) = (L * v) / (v * v + (omega * L) * (omega * L)); - } - - return jacobian; -} \ No newline at end of file diff --git a/src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp b/src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp deleted file mode 100644 index f337c242b..000000000 --- a/src/motion_lib/src/s2v_model/no_wss_bicycle_model.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "motion_lib/s2v_model/no_wss_bicycle_model.hpp" - -std::pair NoWSSBicycleModel::wheels_velocities_to_cg(double rl_rpm, - [[maybe_unused]] double fl_rpm, - double rr_rpm, - [[maybe_unused]] double fr_rpm, - double steering_angle) { - std::pair velocities = {0, 0}; - double rl_velocity = - common_lib::maths::get_wheel_velocity_from_rpm(rl_rpm, this->car_parameters_.wheel_diameter); - double rr_velocity = - common_lib::maths::get_wheel_velocity_from_rpm(rr_rpm, this->car_parameters_.wheel_diameter); - if (steering_angle == 0) { // If no steering angle, moving straight - velocities.first = (rl_velocity + rr_velocity) / 2; - } else if (steering_angle > 0) { - double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); - velocities.second = - rl_velocity / (rear_axis_center_rotation_radius - (this->car_parameters_.track_width / 2)); - velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + - pow(this->car_parameters_.rear_axis_to_camera, 2)) * - fabs(velocities.second); - } else { - double rear_axis_center_rotation_radius = this->car_parameters_.wheelbase / tan(steering_angle); - velocities.second = - rr_velocity / (rear_axis_center_rotation_radius + (this->car_parameters_.track_width / 2)); - velocities.first = sqrt(pow(rear_axis_center_rotation_radius, 2) + - pow(this->car_parameters_.rear_axis_to_camera, 2)) * - fabs(velocities.second); - } - return velocities; -} - -common_lib::structures::Position NoWSSBicycleModel::rear_axis_position( - const common_lib::structures::Position& cg, double orientation, double dist_cg_2_rear_axis) { - common_lib::structures::Position rear_axis; - rear_axis.x = cg.x - dist_cg_2_rear_axis * cos(orientation); - rear_axis.y = cg.y - dist_cg_2_rear_axis * sin(orientation); - return rear_axis; -} - -Eigen::VectorXd NoWSSBicycleModel::cg_velocity_to_wheels(const Eigen::Vector3d& cg_velocities) { - const double vx = cg_velocities(0); - const double omega = cg_velocities(2); - double steering_angle = - (std::fabs(vx) <= 1e-2) ? 0 : atan(omega * (this->car_parameters_.wheelbase) / vx); - double motor_rpm = - 60 * this->car_parameters_.gear_ratio * vx / (M_PI * this->car_parameters_.wheel_diameter); - - Eigen::VectorXd observations = Eigen::VectorXd::Zero(2); - observations << steering_angle, motor_rpm; - return observations; -} - -Eigen::MatrixXd NoWSSBicycleModel::jacobian_cg_velocity_to_wheels( - const Eigen::Vector3d& cg_velocities) { - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(2, 3); - double vx = cg_velocities(0); - double omega = cg_velocities(2); - double common_term = (std::fabs(vx) <= 1e-2) - ? 0 - : 1 / (1 + pow((this->car_parameters_.wheelbase * omega) / vx, 2)); - - jacobian(0, 0) = - common_term * - (std::fabs(vx) <= 1e-2 ? 0 : -this->car_parameters_.wheelbase * omega / (vx * vx)); - jacobian(0, 1) = 0; - jacobian(0, 2) = common_term * (std::fabs(vx) <= 1e-2 ? 0 : this->car_parameters_.wheelbase / vx); - jacobian(1, 0) = - 60 * this->car_parameters_.gear_ratio / (M_PI * this->car_parameters_.wheel_diameter); - jacobian(1, 1) = 0; - jacobian(1, 2) = 0; - return jacobian; -} \ No newline at end of file diff --git a/src/motion_lib/src/steering_model/parallel_front_steering.cpp b/src/motion_lib/src/steering_model/parallel_front_steering.cpp new file mode 100644 index 000000000..3fa4d1dca --- /dev/null +++ b/src/motion_lib/src/steering_model/parallel_front_steering.cpp @@ -0,0 +1,6 @@ +#include "motion_lib/steering_model/parallel_front_steering.hpp" + +Eigen::Vector4d ParallelFrontSteering::calculate_steering_angles( + double steering_wheel_angle) const { + return Eigen::Vector4d(steering_wheel_angle, steering_wheel_angle, 0, 0); +} \ No newline at end of file diff --git a/src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp b/src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp new file mode 100644 index 000000000..669d949ce --- /dev/null +++ b/src/motion_lib/src/steering_motor_model/pid_steering_motor.cpp @@ -0,0 +1,24 @@ +#include "motion_lib/steering_motor_model/pid_steering_motor.hpp" + +double PIDSteeringMotor::compute_steering_rate(double current_steering, double steering_goal) { + if (this->last_update == rclcpp::Time(0, 0)) { + this->last_update = rclcpp::Clock().now(); + return 0; + } + double dt = (rclcpp::Clock().now() - this->last_update).seconds(); + double error = steering_goal - current_steering; + + double proportional = error; + + integral_ += error * dt; + + double derivative = (error - previous_error_) / dt; + previous_error_ = error; + + // PID control output + double output = this->car_parameters_->steering_motor_parameters.kp * proportional + + this->car_parameters_->steering_motor_parameters.ki * integral_ + + this->car_parameters_->steering_motor_parameters.kd * derivative; + + return output; +} \ No newline at end of file diff --git a/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp b/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp new file mode 100644 index 000000000..e78597085 --- /dev/null +++ b/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp @@ -0,0 +1,32 @@ +#include "motion_lib/tire_model/pacejka_combined_slip.hpp" + +std::pair PacejkaCombinedSlip::tire_forces(double slip_angle, double slip_ratio, + double vertical_load) const { + // Longitudinal pure slip force (Fx0) + double Bx = this->car_parameters_->tire_parameters.tire_B_longitudinal; + double Cx = this->car_parameters_->tire_parameters.tire_C_longitudinal; + double Dx = this->car_parameters_->tire_parameters.tire_D_longitudinal * vertical_load; + double Ex = this->car_parameters_->tire_parameters.tire_E_longitudinal; + + double Fx0 = Dx * std::sin(Cx * std::atan(Bx * slip_ratio - + Ex * (Bx * slip_ratio - std::atan(Bx * slip_ratio)))); + + // Lateral pure slip force (Fy0) + double By = this->car_parameters_->tire_parameters.tire_B_lateral; + double Cy = this->car_parameters_->tire_parameters.tire_C_lateral; + double Dy = this->car_parameters_->tire_parameters.tire_D_lateral * vertical_load; + double Ey = this->car_parameters_->tire_parameters.tire_E_lateral; + + double Fy0 = Dy * std::sin(Cy * std::atan(By * slip_angle - + Ey * (By * slip_angle - std::atan(By * slip_angle)))); + + // Combined slip reduction factors (friction ellipse type) + double Gx = std::cos(std::atan(By * slip_angle)); + double Gy = std::cos(std::atan(Bx * slip_ratio)); + + // Final forces with combined slip + double Fx = Fx0 * Gx; + double Fy = Fy0 * Gy; + + return std::make_pair(Fx, Fy); +} \ No newline at end of file From fa3ce751f6efad8d9001a12fdaf58a157b019145 Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Mon, 24 Nov 2025 16:37:13 +0000 Subject: [PATCH 04/10] added cviz --- cone_viz/cone_viz/__init__.py | 0 cone_viz/cone_viz/cone_marker_publisher.py | 100 +++++++++++++++++++++ cone_viz/package.xml | 23 +++++ cone_viz/resource/cone_viz | 0 cone_viz/setup.cfg | 4 + cone_viz/setup.py | 20 +++++ cone_viz/test/test_copyright.py | 25 ++++++ cone_viz/test/test_flake8.py | 25 ++++++ cone_viz/test/test_pep257.py | 23 +++++ 9 files changed, 220 insertions(+) create mode 100644 cone_viz/cone_viz/__init__.py create mode 100644 cone_viz/cone_viz/cone_marker_publisher.py create mode 100644 cone_viz/package.xml create mode 100644 cone_viz/resource/cone_viz create mode 100644 cone_viz/setup.cfg create mode 100644 cone_viz/setup.py create mode 100644 cone_viz/test/test_copyright.py create mode 100644 cone_viz/test/test_flake8.py create mode 100644 cone_viz/test/test_pep257.py diff --git a/cone_viz/cone_viz/__init__.py b/cone_viz/cone_viz/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cone_viz/cone_viz/cone_marker_publisher.py b/cone_viz/cone_viz/cone_marker_publisher.py new file mode 100644 index 000000000..ab5f182ad --- /dev/null +++ b/cone_viz/cone_viz/cone_marker_publisher.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +import math +import rclpy +from rclpy.node import Node + +from visualization_msgs.msg import Marker, MarkerArray +from custom_interfaces.msg import PerceptionOutput, Pose + + +class ConeMarkerPublisher(Node): + def __init__(self): + super().__init__("cone_marker_publisher") + + self.vehicle_x = 0.0 + self.vehicle_y = 0.0 + self.vehicle_theta = 0.0 + self.have_pose = False + + # Store all global cones seen so far + self.global_cones = [] + + self.create_subscription( + PerceptionOutput, "/perception/cones", self.cones_callback, 10 + ) + + self.create_subscription( + Pose, "/state_estimation/vehicle_pose", self.pose_callback, 10 + ) + + self.marker_pub = self.create_publisher( + MarkerArray, "/visualization/cones_markers", 10 + ) + + def pose_callback(self, msg: Pose): + self.vehicle_x = msg.x + self.vehicle_y = msg.y + self.vehicle_theta = msg.theta + self.have_pose = True + + def cones_callback(self, msg: PerceptionOutput): + if not self.have_pose: + return + + cos_t = math.cos(self.vehicle_theta) + sin_t = math.sin(self.vehicle_theta) + + # Transform new cones to global frame and store them + for cone in msg.cone_array: + lx = cone.x + ly = cone.y + + gx = self.vehicle_x + cos_t * lx - sin_t * ly + gy = self.vehicle_y + sin_t * lx + cos_t * ly + + self.global_cones.append((gx, gy)) + + # Build the map markers with ALL cones observed so far + marker_array = MarkerArray() + marker_id = 0 + + for gx, gy in self.global_cones: + marker = Marker() + marker.header.frame_id = "map" + marker.header.stamp = self.get_clock().now().to_msg() + marker.id = marker_id + marker.type = Marker.SPHERE + + marker.pose.position.x = gx + marker.pose.position.y = gy + marker.pose.position.z = 0.0 + + marker.scale.x = 0.25 + marker.scale.y = 0.25 + marker.scale.z = 0.25 + + marker.color.r = 0.0 + marker.color.g = 0.5 + marker.color.b = 1.0 + marker.color.a = 1.0 + + # Make markers persistent + marker.lifetime.sec = 0 + marker.lifetime.nanosec = 0 + + marker_array.markers.append(marker) + marker_id += 1 + + self.marker_pub.publish(marker_array) + + +def main(args=None): + rclpy.init(args=args) + node = ConeMarkerPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/cone_viz/package.xml b/cone_viz/package.xml new file mode 100644 index 000000000..8638187d1 --- /dev/null +++ b/cone_viz/package.xml @@ -0,0 +1,23 @@ + + + + cone_viz + 0.0.0 + TODO: Package description + filipelopes + TODO: License declaration + + rclpy + visualization_msgs + custom_interfaces + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/cone_viz/resource/cone_viz b/cone_viz/resource/cone_viz new file mode 100644 index 000000000..e69de29bb diff --git a/cone_viz/setup.cfg b/cone_viz/setup.cfg new file mode 100644 index 000000000..5ffe35e22 --- /dev/null +++ b/cone_viz/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/cone_viz +[install] +install_scripts=$base/lib/cone_viz diff --git a/cone_viz/setup.py b/cone_viz/setup.py new file mode 100644 index 000000000..6c3e84d38 --- /dev/null +++ b/cone_viz/setup.py @@ -0,0 +1,20 @@ +from setuptools import setup + +package_name = "cone_viz" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Your Name", + maintainer_email="you@example.com", + description="Visualize cones from perception and vehicle pose", + license="Apache-2.0", + entry_points={ + "console_scripts": [ + "cone_marker_publisher = cone_viz.cone_marker_publisher:main", + ], + }, +) diff --git a/cone_viz/test/test_copyright.py b/cone_viz/test/test_copyright.py new file mode 100644 index 000000000..97a39196e --- /dev/null +++ b/cone_viz/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/cone_viz/test/test_flake8.py b/cone_viz/test/test_flake8.py new file mode 100644 index 000000000..27ee1078f --- /dev/null +++ b/cone_viz/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/cone_viz/test/test_pep257.py b/cone_viz/test/test_pep257.py new file mode 100644 index 000000000..b234a3840 --- /dev/null +++ b/cone_viz/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 04099e2d5e405c83583bb23bed0121ae5a24eeaf Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Fri, 28 Nov 2025 01:28:10 +0000 Subject: [PATCH 05/10] pose compensation + deskew + kinda working not reliable --- config/perception/vehicle.yaml | 2 +- config/slam/vehicle.yaml | 6 +- .../car_parameters/car_parameters.hpp | 2 +- .../common_lib/structures/circular_buffer.hpp | 68 ++++++++++++++++ .../include/perception/perception_node.hpp | 2 +- src/perception/src/deskew/deskew.cpp | 26 +++---- .../src/perception/perception_node.cpp | 12 ++- .../include/slam_config/general_config.hpp | 2 + .../include/slam_solver/ekf_slam_solver.hpp | 3 +- .../graph_slam_solver/graph_slam_instance.hpp | 15 +++- .../graph_slam_solver/graph_slam_solver.hpp | 4 +- .../pose_updater/base_pose_updater.hpp | 64 ++++++++------- src/slam/include/slam_solver/slam_solver.hpp | 3 +- src/slam/src/ros_node/slam_node.cpp | 21 ++--- src/slam/src/slam_solver/ekf_slam_solver.cpp | 3 +- .../graph_slam_solver/graph_slam_instance.cpp | 45 +++++++++-- .../graph_slam_solver/graph_slam_solver.cpp | 17 ++-- .../optimizer/isam2_optimizer.cpp | 30 +++++-- .../pose_updater/base_pose_updater.cpp | 78 ++++++++++++------- .../graph_slam_solver_test.cpp | 4 +- 20 files changed, 281 insertions(+), 126 deletions(-) create mode 100644 src/common_lib/include/common_lib/structures/circular_buffer.hpp diff --git a/config/perception/vehicle.yaml b/config/perception/vehicle.yaml index 9eb2839f5..174d9d3dc 100644 --- a/config/perception/vehicle.yaml +++ b/config/perception/vehicle.yaml @@ -8,7 +8,7 @@ perception: #FOV trimming apply_rotation: false - rotation: 90.0 + rotation: 2.736 apply_fov_trimming: false fov: 180.0 min_range: 2.5 diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index ab5ba4098..f59ae0d8d 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -7,7 +7,7 @@ slam: data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model data_association_limit_distance: 70 # maximum distance to consider a cone for data association data_association_gate: 1.5 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark - new_landmark_confidence_gate: 0.6 # Minimum confidence to consider an observation as a new landmark in data asociation + new_landmark_confidence_gate: 0.0 # Minimum confidence to consider an observation as a new landmark in data asociation observation_x_noise: 0.02 # sigma of the noise for cone positions in x (range in Graph SLAM) observation_y_noise: 0.001 # sigma of the noise for cone positions in y (bearing in Graph SLAM) velocity_x_noise: 0.1 # variance of the noise for the velocity in x @@ -28,10 +28,10 @@ slam: slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph # Only for slam_optimization_mode: "async" - slam_optimization_period: 0.2 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added + slam_optimization_period: 0.25 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added # Only for slam_optimization_type: "isam2" - slam_isam2_relinearize_threshold: 0.1 # Threshold for relinearizing the graph in isam2 + slam_isam2_relinearize_threshold: 0.05 # Threshold for relinearizing the graph in isam2 slam_isam2_relinearize_skip: 5 # Number of steps to skip before relinearizing the graph in isam2 slam_isam2_factorization: "QR" # "QR" or "CHOLESKY" -> Factorization method for isam2 diff --git a/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp b/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp index 2ada73d31..659ae273f 100644 --- a/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp +++ b/src/common_lib/include/common_lib/car_parameters/car_parameters.hpp @@ -15,9 +15,9 @@ namespace common_lib::car_parameters { struct CarParameters { double wheel_diameter = 0.406; double wheelbase = 1.530; + double rear_axis_to_camera = 0.79; double track_width = 1.2; double dist_cg_2_rear_axis = 0.804; - double rear_axis_to_camera = 0.79; double gear_ratio = 4; double cog_height = 0.5; diff --git a/src/common_lib/include/common_lib/structures/circular_buffer.hpp b/src/common_lib/include/common_lib/structures/circular_buffer.hpp new file mode 100644 index 000000000..2d196fec9 --- /dev/null +++ b/src/common_lib/include/common_lib/structures/circular_buffer.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include + +template +class CircularBuffer { +public: + explicit CircularBuffer(size_t capacity = 0) + : capacity_(capacity), head_(0), size_(0), buffer_(capacity) {} + + void set_capacity(size_t capacity) { + capacity_ = capacity; + buffer_.resize(capacity); + clear(); + } + + void clear() { + head_ = 0; + size_ = 0; + } + + inline size_t size() const { return size_; } + inline size_t capacity() const { return capacity_; } + inline bool empty() const { return size_ == 0; } + inline bool full() const { return size_ == capacity_; } + + /** Add item into circular buffer */ + void push(const T& item) { + if (capacity_ == 0) return; + + buffer_[head_] = item; + head_ = (head_ + 1) % capacity_; + + if (size_ < capacity_) size_++; + } + + /** Access the most recent item */ + const T& latest() const { + if (size_ == 0) throw std::out_of_range("CircularBuffer::latest - buffer empty"); + + size_t index = (head_ + capacity_ - 1) % capacity_; + return buffer_[index]; + } + + /** Access i-th element from the end (0 = latest, 1 = previous, ...) */ + const T& from_end(size_t i) const { + if (i >= size_) throw std::out_of_range("CircularBuffer::from_end - index too large"); + + size_t index = (head_ + capacity_ - 1 - i) % capacity_; + return buffer_[index]; + } + + /** Iterate raw access */ + const T& operator[](size_t index) const { + if (index >= size_) throw std::out_of_range("CircularBuffer::operator[] - index too large"); + + size_t real_index = (head_ + capacity_ - size_ + index) % capacity_; + return buffer_[real_index]; + } + +private: + size_t capacity_; + size_t head_; + size_t size_; + std::vector buffer_; +}; diff --git a/src/perception/include/perception/perception_node.hpp b/src/perception/include/perception/perception_node.hpp index b6b7fceed..9c1679801 100644 --- a/src/perception/include/perception/perception_node.hpp +++ b/src/perception/include/perception/perception_node.hpp @@ -142,7 +142,7 @@ class Perception : public rclcpp::Node { * @param cones A reference to a vector of Cluster objects representing the clusters (cones) * to be published. */ - void publish_cones(std::vector* cones, double exec_time); + void publish_cones(std::vector* cones, double exec_time, rclcpp::Time pcl_time); void velocities_callback(const custom_interfaces::msg::Velocities& msg); diff --git a/src/perception/src/deskew/deskew.cpp b/src/perception/src/deskew/deskew.cpp index ca4e0e8ae..c749402ec 100644 --- a/src/perception/src/deskew/deskew.cpp +++ b/src/perception/src/deskew/deskew.cpp @@ -18,13 +18,17 @@ void Deskew::deskew_point_cloud(sensor_msgs::msg::PointCloud2::SharedPtr& input_ auto& cloud = input_cloud->data; - // Reference point (last in scan) - int ref_index = static_cast(num_points - 1); - double ref_x = *reinterpret_cast(&cloud[LidarPoint::PointX(ref_index)]); - double ref_y = *reinterpret_cast(&cloud[LidarPoint::PointY(ref_index)]); - double reference_azimuth = std::atan2(ref_x, ref_y); - if (reference_azimuth < 0.0) { - reference_azimuth += 2.0 * M_PI; + // Reference point + double ref_x; + double ref_y; + double reference_azimuth = std::numeric_limits::lowest(); + for (size_t i = 0; i < num_points; ++i) { + ref_x = *reinterpret_cast(&cloud[LidarPoint::PointX(i)]); + ref_y = *reinterpret_cast(&cloud[LidarPoint::PointY(i)]); + double azimuth = -std::atan2(ref_y, ref_x); + if (azimuth > reference_azimuth) { + reference_azimuth = azimuth; + } } // Deskew each point @@ -33,15 +37,9 @@ void Deskew::deskew_point_cloud(sensor_msgs::msg::PointCloud2::SharedPtr& input_ float y = *reinterpret_cast(&cloud[LidarPoint::PointY(i)]); float z = *reinterpret_cast(&cloud[LidarPoint::PointZ(i)]); - double azimuth = std::atan2(x, y); - if (azimuth < 0.0) { - azimuth += 2.0 * M_PI; - } + double azimuth = -std::atan2(y, x); double delta_angle = reference_azimuth - azimuth; - if (delta_angle < 0.0) { - delta_angle += 2.0 * M_PI; - } double time_offset = delta_angle * one_over_two_pi * scan_duration; diff --git a/src/perception/src/perception/perception_node.cpp b/src/perception/src/perception/perception_node.cpp index 1c26a07d5..2e938ae5f 100644 --- a/src/perception/src/perception/perception_node.cpp +++ b/src/perception/src/perception/perception_node.cpp @@ -243,6 +243,12 @@ Perception::Perception(const PerceptionParameters& params) } void Perception::point_cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + // Get start perception time + // rclcpp::Time pcl_time = msg->header.stamp; + rclcpp::Time pcl_time = + this->now() - rclcpp::Duration::from_seconds(0.008); // approximate time delay + + // Reset lidar off timer this->lidar_off_timer_ = this->create_wall_timer( std::chrono::milliseconds(2000), std::bind(&Perception::lidar_timer_callback, this)); @@ -308,13 +314,15 @@ void Perception::point_cloud_callback(const sensor_msgs::msg::PointCloud2::Share exec_time_msg.data = *(this->_execution_times_); this->_perception_execution_time_publisher_->publish(exec_time_msg); - publish_cones(&filtered_clusters, perception_execution_time_seconds); + publish_cones(&filtered_clusters, perception_execution_time_seconds, pcl_time); } -void Perception::publish_cones(std::vector* cones, double exec_time) { +void Perception::publish_cones(std::vector* cones, double exec_time, + rclcpp::Time pcl_time) { auto message = custom_interfaces::msg::PerceptionOutput(); std::vector message_array = {}; message.header = ::header; + message.header.stamp = pcl_time; for (int i = 0; i < static_cast(cones->size()); i++) { auto position = custom_interfaces::msg::Point2d(); position.x = cones->at(i).get_centroid().x(); diff --git a/src/slam/include/slam_config/general_config.hpp b/src/slam/include/slam_config/general_config.hpp index cb695cc02..9c5231470 100644 --- a/src/slam/include/slam_config/general_config.hpp +++ b/src/slam/include/slam_config/general_config.hpp @@ -15,6 +15,8 @@ struct SLAMParameters { std::string motion_model_name_ = "constant_velocity"; std::string pose_updater_name_ = "base_pose_updater"; // Name of the pose updater object, // responsible for keeping pose estimate + int max_pose_history_updater = 30; // Maximum number of poses to keep in the pose buffer + int max_pose_history_graph = 50; // Maximum number of graphed poses to keep in the graph buffer std::string data_association_model_name_ = "nearest_neighbor"; std::string lidar_odometry_topic_ = "/fast_limo/state"; // Topic for pose from lidar odometry bool receive_lidar_odometry_ = false; // Whether to use lidar odometry topic or not diff --git a/src/slam/include/slam_solver/ekf_slam_solver.hpp b/src/slam/include/slam_solver/ekf_slam_solver.hpp index 86cb5a475..a9f861865 100644 --- a/src/slam/include/slam_solver/ekf_slam_solver.hpp +++ b/src/slam/include/slam_solver/ekf_slam_solver.hpp @@ -118,7 +118,8 @@ class EKFSLAMSolver : public SLAMSolver, public VelocitiesIntegratorTrait { * * @param position */ - void add_observations(const std::vector& positions) override; + void add_observations(const std::vector& positions, + rclcpp::Time cones_timestamp) override; /** * @brief Initialize the EKF SLAM solver with a previously saved map and pose diff --git a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp index 26d0b6c2d..3effa2c3d 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_instance.hpp @@ -5,6 +5,7 @@ #include #include +#include "common_lib/structures/circular_buffer.hpp" #include "slam_config/general_config.hpp" #include "slam_solver/graph_slam_solver/factor_data_structures.hpp" #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp" @@ -16,6 +17,11 @@ */ class GraphSLAMInstance { protected: + struct TimedPose { + unsigned int pose_id; + rclcpp::Time timestamp; + }; + gtsam::NonlinearFactorGraph _factor_graph_; //< Factor graph for the graph SLAM solver (only factors, no estimates) gtsam::Values @@ -30,6 +36,11 @@ class GraphSLAMInstance { SLAMParameters _params_; //< Parameters for the SLAM solver std::shared_ptr _optimizer_; //< Optimizer for the graph SLAM solver + CircularBuffer + _pose_timestamps_; //< Buffer to hold the timestamps of the poses added to the graph + + unsigned int get_landmark_id_at_time(const rclcpp::Time& timestamp) const; + public: GraphSLAMInstance() = default; @@ -132,7 +143,7 @@ class GraphSLAMInstance { * @param new_pose The new pose to add to the graph */ void process_new_pose(const Eigen::Vector3d& pose_difference, const Eigen::Vector3d& noise_vector, - const Eigen::Vector3d& new_pose); + const Eigen::Vector3d& new_pose, const rclcpp::Time& pose_timestamp); /** * @brief Process a pose difference and add the respective factor to the graph @@ -164,6 +175,8 @@ class GraphSLAMInstance { void process_pose_difference(const Eigen::Vector3d& pose_difference, const Eigen::Vector3d& noise_vector); + void ensure_all_keys_have_initials(); + /** * @brief Runs optimization on the graph * @details This method calls the optimizer to run optimization on the current factor graph diff --git a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp index 1cc8d5552..f62feebed 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp @@ -133,8 +133,10 @@ class GraphSLAMSolver : public SLAMSolver, * @brief Add observations to the solver (correction step) * * @param cones Positions of the observations + * @param cones_timestamp Timestamp of the observations */ - void add_observations(const std::vector& cones) override; + void add_observations(const std::vector& cones, + rclcpp::Time cones_timestamp) override; /** * @brief Initialize the graph SLAM solver with a previously saved map and pose diff --git a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp index 4d25bf13f..9ffd7f011 100644 --- a/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp @@ -1,8 +1,10 @@ #pragma once #include +#include #include +#include "common_lib/structures/circular_buffer.hpp" #include "motion_lib/v2p_models/base_v2p_motion_model.hpp" #include "slam_config/general_config.hpp" #include "slam_solver/graph_slam_solver/factor_data_structures.hpp" @@ -15,31 +17,36 @@ */ class PoseUpdater { protected: - Eigen::Vector3d _last_pose_; ///< Last estimated pose (x, y, theta) - Eigen::Vector3d _last_graphed_pose_ = Eigen::Vector3d::Zero(); ///< Last pose added to the graph - Eigen::Matrix3d _last_pose_covariance_ = - Eigen::Matrix3d::Zero(); ///< Covariance of the pose difference up to the last pose - rclcpp::Time _last_pose_update_ = rclcpp::Time(0); ///< Timestamp of the last pose update + struct TimedPose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d pose; + rclcpp::Time timestamp; - bool _received_first_motion_data_ = - false; ///< Flag to indicate if the first motion data has been received - bool _new_pose_from_graph_ = false; ///< Flag to indicate if the pose was updated from the graph + TimedPose() : pose(Eigen::Vector3d::Zero()), timestamp(rclcpp::Time(0)) {} + TimedPose(const Eigen::Vector3d& p, const rclcpp::Time& t) : pose(p), timestamp(t) {} + }; + + Eigen::Vector3d _last_pose_; + Eigen::Vector3d _last_graphed_pose_; + Eigen::Matrix3d _last_pose_covariance_; + rclcpp::Time _last_pose_update_; + + bool _received_first_motion_data_ = false; + bool _new_pose_from_graph_ = false; + + CircularBuffer _pose_buffer_; public: explicit PoseUpdater(const SLAMParameters& params); - PoseUpdater(const PoseUpdater& other); - PoseUpdater& operator=(const PoseUpdater& other); - virtual ~PoseUpdater(); /** - * @brief Clone the pose updater - * @details This method is used to create a copy of the pose updater - * It is useful for polymorphic classes that use pointers to base class - * - * @return A shared pointer to the cloned pose updater + * @brief Class to update the pose of the vehicle + * @details This class is the one to apply the motion model and keep track of the most up to date + * pose. It allows for the pose estimate to be calculated independently of the graph optimization, + * and only update the graph when needed. */ virtual std::shared_ptr clone() const; @@ -67,31 +74,22 @@ class PoseUpdater { */ virtual bool pose_ready_for_graph_update() const; - /** @brief Get the adjoint operator matrix for a given pose - * @details The adjoint operator matrix is used to transform the covariance - * of the pose difference when applying the motion model. It accounts for the - * rotation and translation of the pose. - * @param x_translation The x translation - * @param y_translation The y translation - * @param rotation_angle The rotation angle - * @return The adjoint operator matrix - */ - Eigen::Matrix3d get_adjoint_operator_matrix(const double x_translation, - const double y_translation, - const double rotation_angle) const; + Eigen::Matrix3d get_adjoint_operator_matrix(double x_translation, double y_translation, + double rotation_angle) const; Eigen::Vector3d get_last_pose() const { return _last_pose_; } Eigen::Vector3d get_last_graphed_pose() const { return _last_graphed_pose_; } - /** - * @brief Get the pose difference noise as standard deviation - * @return The pose difference noise (standard deviation for x, y, theta) - */ Eigen::Vector3d get_pose_difference_noise() const { // Return the square root of the diagonal as standard deviation return _last_pose_covariance_.diagonal().array().sqrt(); } rclcpp::Time get_last_pose_update() const { return _last_pose_update_; } -}; \ No newline at end of file + + inline size_t pose_buffer_size() const { return _pose_buffer_.size(); } + inline size_t pose_buffer_capacity() const { return _pose_buffer_.capacity(); } + + Eigen::Vector3d get_pose_at_timestamp(const rclcpp::Time& timestamp) const; +}; diff --git a/src/slam/include/slam_solver/slam_solver.hpp b/src/slam/include/slam_solver/slam_solver.hpp index f573cdc38..bc078deaf 100644 --- a/src/slam/include/slam_solver/slam_solver.hpp +++ b/src/slam/include/slam_solver/slam_solver.hpp @@ -62,7 +62,8 @@ class SLAMSolver { * * @param cones Positions of the observations */ - virtual void add_observations(const std::vector& cones) = 0; + virtual void add_observations(const std::vector& cones, + rclcpp::Time cones_timestamp) = 0; /** * @brief Loads a previously saved map and pose into the solver diff --git a/src/slam/src/ros_node/slam_node.cpp b/src/slam/src/ros_node/slam_node.cpp index d36b801fb..13b8d6c65 100644 --- a/src/slam/src/ros_node/slam_node.cpp +++ b/src/slam/src/ros_node/slam_node.cpp @@ -110,7 +110,7 @@ void SLAMNode::init() { void SLAMNode::_perception_subscription_callback( const custom_interfaces::msg::PerceptionOutput &msg) { auto const &cone_array = msg.cones.cone_array; - auto const perception_exec_time = msg.exec_time; + rclcpp::Time const cones_time = msg.header.stamp; RCLCPP_DEBUG(this->get_logger(), "SUB - Perception: %ld cones. Mission: %d", cone_array.size(), static_cast(this->_mission_)); @@ -128,23 +128,12 @@ void SLAMNode::_perception_subscription_callback( } this->_perception_map_.clear(); - - // Compensate for motion during perception delay (See if now is needed) - double theta = -this->_vehicle_state_velocities_.rotational_velocity * perception_exec_time; - double cos_theta = std::cos(theta); - double sin_theta = std::sin(theta); for (auto &cone : cone_array) { - double x_linear_compensated = - cone.position.x - this->_vehicle_state_velocities_.velocity_x * perception_exec_time; - double y_linear_compensated = - cone.position.y - this->_vehicle_state_velocities_.velocity_y * perception_exec_time; - double x_compensated = cos_theta * x_linear_compensated - sin_theta * y_linear_compensated; - double y_compensated = sin_theta * x_linear_compensated + cos_theta * y_linear_compensated; this->_perception_map_.push_back(common_lib::structures::Cone( - x_compensated, y_compensated, cone.color, cone.confidence, msg.header.stamp)); + cone.position.x, cone.position.y, cone.color, cone.confidence, msg.header.stamp)); } - this->_slam_solver_->add_observations(this->_perception_map_); + this->_slam_solver_->add_observations(this->_perception_map_, cones_time); this->_track_map_ = this->_slam_solver_->get_map_estimate(); this->_vehicle_pose_ = this->_slam_solver_->get_pose_estimate(); this->_associations_ = this->_slam_solver_->get_associations(); @@ -183,6 +172,8 @@ void SLAMNode::_velocities_subscription_callback(const custom_interfaces::msg::V msg.velocity_x, msg.velocity_y, msg.angular_velocity, msg.covariance[0], msg.covariance[4], msg.covariance[8], msg.header.stamp); + // this->_vehicle_state_velocities_.timestamp_ = this->get_clock()->now() + + // rclcpp::Duration::from_seconds(0.003); solver_ptr->add_velocities(this->_vehicle_state_velocities_); } this->_vehicle_pose_ = this->_slam_solver_->get_pose_estimate(); @@ -208,6 +199,8 @@ void SLAMNode::_imu_subscription_callback(const sensor_msgs::msg::Imu &msg) { msg.header.stamp, msg.angular_velocity_covariance[8], msg.linear_acceleration_covariance[0], msg.linear_acceleration_covariance[4]); + imu_data.timestamp_ = this->get_clock()->now(); + solver_ptr->add_imu_data(imu_data); } } diff --git a/src/slam/src/slam_solver/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp index dec7dd463..bacfdbf20 100644 --- a/src/slam/src/slam_solver/ekf_slam_solver.cpp +++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp @@ -39,7 +39,8 @@ void EKFSLAMSolver::add_velocities(const common_lib::structures::Velocities& vel this->last_update_ = velocities.timestamp_; } -void EKFSLAMSolver::add_observations(const std::vector& cones) { +void EKFSLAMSolver::add_observations(const std::vector& cones, + rclcpp::Time cones_timestamp) { // Prepare data structures int num_observations = static_cast(cones.size()); std::vector matched_landmarks_indices; diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp index 5a701431c..7541c186a 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp @@ -77,7 +77,7 @@ Eigen::MatrixXd GraphSLAMInstance::get_covariance_matrix() const { GraphSLAMInstance::GraphSLAMInstance(const SLAMParameters& params, std::shared_ptr optimizer) - : _params_(params), _optimizer_(optimizer) { + : _params_(params), _optimizer_(optimizer), _pose_timestamps_(params.max_pose_history_graph) { // Create a new factor graph _factor_graph_ = gtsam::NonlinearFactorGraph(); const gtsam::Pose2 prior_pose(0.0, 0.0, @@ -108,6 +108,7 @@ GraphSLAMInstance::GraphSLAMInstance(const GraphSLAMInstance& other) { _new_observation_factors_ = other._new_observation_factors_; _optimizer_ = other._optimizer_->clone(); // because it's a shared_ptr _params_ = other._params_; + _pose_timestamps_ = other._pose_timestamps_; } GraphSLAMInstance& GraphSLAMInstance::operator=(const GraphSLAMInstance& other) { @@ -122,13 +123,15 @@ GraphSLAMInstance& GraphSLAMInstance::operator=(const GraphSLAMInstance& other) _new_observation_factors_ = other._new_observation_factors_; _optimizer_ = other._optimizer_->clone(); // because it's a shared_ptr _params_ = other._params_; + _pose_timestamps_ = other._pose_timestamps_; return *this; } void GraphSLAMInstance::process_new_pose(const Eigen::Vector3d& pose_difference, const Eigen::Vector3d& noise_vector, - const Eigen::Vector3d& new_pose) { + const Eigen::Vector3d& new_pose, + const rclcpp::Time& pose_timestamp) { RCLCPP_DEBUG(rclcpp::get_logger("slam"), "GraphSLAMInstance - Processing new pose %lf %lf %lf", new_pose(0), new_pose(1), new_pose(2)); gtsam::Pose2 new_pose_gtsam = eigen_to_gtsam_pose(new_pose); @@ -136,6 +139,9 @@ void GraphSLAMInstance::process_new_pose(const Eigen::Vector3d& pose_difference, // X means pose node, l means landmark node gtsam::Symbol new_pose_symbol('x', ++(this->_pose_counter_)); + // Store the timestamp of the new pose in the circular buffer + this->_pose_timestamps_.push(TimedPose{this->_pose_counter_, pose_timestamp}); + // Add the prior pose to the values _graph_values_.insert(new_pose_symbol, new_pose_gtsam); this->_new_pose_node_ = true; @@ -225,13 +231,14 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { optimizer_ptr->_new_factors_.add(gtsam::BearingRangeFactor( - gtsam::Symbol('x', this->_pose_counter_), landmark_symbol, observation_rotation, - observation_cylindrical(0), observation_noise)); // Again, just for ISAM2, because - // it's incremental + gtsam::Symbol('x', this->get_landmark_id_at_time(observation_data.timestamp_)), + landmark_symbol, observation_rotation, observation_cylindrical(0), + observation_noise)); // Again, just for ISAM2, because + // it's incremental } this->_factor_graph_.add(gtsam::BearingRangeFactor( - gtsam::Symbol('x', this->_pose_counter_), landmark_symbol, observation_rotation, - observation_cylindrical(0), observation_noise)); + gtsam::Symbol('x', this->get_landmark_id_at_time(observation_data.timestamp_)), + landmark_symbol, observation_rotation, observation_cylindrical(0), observation_noise)); } this->_new_pose_node_ = !new_observation_factors; this->_new_observation_factors_ = new_observation_factors; @@ -290,4 +297,26 @@ void GraphSLAMInstance::lock_landmarks(double locked_landmark_noise) { gtsam::PriorFactor(landmark_symbol, landmark, landmark_noise)); } } -} \ No newline at end of file +} + +unsigned int GraphSLAMInstance::get_landmark_id_at_time(const rclcpp::Time& timestamp) const { + if (_pose_timestamps_.size() == 0) { + throw std::runtime_error("No poses in buffer"); + } + + for (size_t i = 0; i < _pose_timestamps_.size(); ++i) { + const TimedPose& curr = _pose_timestamps_.from_end(i); + + if (curr.timestamp <= timestamp) { + if (i == 0) return curr.pose_id; // AVoid out-of-bounds + const TimedPose& older = _pose_timestamps_.from_end(i - 1); + + auto diff_curr = timestamp.nanoseconds() - curr.timestamp.nanoseconds(); + auto diff_older = older.timestamp.nanoseconds() - timestamp.nanoseconds(); + return (diff_curr <= diff_older) ? curr.pose_id : older.pose_id; + } + } + + // If no pose <= timestamp, return oldest pose + return _pose_timestamps_.from_end(_pose_timestamps_.size() - 1).pose_id; +} diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp index 4d6d42f0c..959b755f0 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_solver.cpp @@ -69,7 +69,8 @@ bool GraphSLAMSolver::_add_motion_data_to_graph( // Eigen::Vector3d pose_difference_noise = {0.2, 0.2, 0.3}; graph_slam_instance->process_new_pose( pose_difference_eigen(pose_updater->get_last_graphed_pose(), pose_updater->get_last_pose()), - pose_updater->get_pose_difference_noise(), pose_updater->get_last_pose()); + pose_updater->get_pose_difference_noise(), pose_updater->get_last_pose(), + pose_updater->get_last_pose_update()); pose_updater->update_pose( pose_updater->get_last_pose()); // Reset the accumulated pose difference @@ -164,7 +165,8 @@ void GraphSLAMSolver::add_imu_data(const common_lib::sensor_data::ImuData& imu_d this->_last_imu_data_ = imu_data; } -void GraphSLAMSolver::add_observations(const std::vector& cones) { +void GraphSLAMSolver::add_observations(const std::vector& cones, + rclcpp::Time cones_timestamp) { if (cones.empty()) { // this->_landmark_filter_->filter(Eigen::VectorXd::Zero(0), Eigen::VectorXd::Zero(0), // Eigen::VectorXi::Zero(0)); @@ -195,8 +197,11 @@ void GraphSLAMSolver::add_observations(const std::vector_mutex_); state = this->_graph_slam_instance_->get_state_vector(); landmarks = state.segment(3, state.size() - 3); - pose = this->_pose_updater_->get_last_pose(); - observations_global = common_lib::maths::local_to_global_coordinates(pose, observations); + pose = this->_pose_updater_->get_last_pose(); // get the last pose + Eigen::Vector3d adjusted_pose = this->_pose_updater_->get_pose_at_timestamp( + cones_timestamp); // get the pose right before observation + observations_global = + common_lib::maths::local_to_global_coordinates(adjusted_pose, observations); initialization_time = rclcpp::Clock().now(); covariance = this->_graph_slam_instance_->get_covariance_matrix(); covariance_time = rclcpp::Clock().now(); @@ -239,7 +244,7 @@ void GraphSLAMSolver::add_observations(const std::vector(associations), std::make_shared(observations_global), std::make_shared(observations_confidences), - cones.at(0).timestamp); + cones_timestamp); if (this->_optimization_under_way_) { this->_observation_data_queue_.push(observation_data); RCLCPP_INFO(rclcpp::get_logger("slam"), @@ -325,7 +330,7 @@ void GraphSLAMSolver::_asynchronous_optimization_routine() { this->_motion_data_queue_.size(), this->_observation_data_queue_.size()); bool process_pose = this->_observation_data_queue_.size() <= 0 || (this->_motion_data_queue_.size() > 0 && - this->_motion_data_queue_.front().timestamp_ < + this->_motion_data_queue_.front().timestamp_ <= this->_observation_data_queue_.front().timestamp_); if (process_pose) { std::shared_ptr motion_model_ptr = diff --git a/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp b/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp index e0e5c86c4..17ad72316 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/optimizer/isam2_optimizer.cpp @@ -52,17 +52,35 @@ std::shared_ptr ISAM2Optimizer::clone() const { return std::make_shared(*this); } -gtsam::Values ISAM2Optimizer::optimize([[maybe_unused]] gtsam::NonlinearFactorGraph& factor_graph, - [[maybe_unused]] gtsam::Values& graph_values, +gtsam::Values ISAM2Optimizer::optimize(gtsam::NonlinearFactorGraph& factor_graph, + gtsam::Values& graph_values, [[maybe_unused]] unsigned int pose_num, [[maybe_unused]] unsigned int landmark_num) { RCLCPP_DEBUG(rclcpp::get_logger("slam"), "ISAM2Optimizer - Adding %zu new factors", factor_graph.size()); + + // Ensure all keys in factor_graph exist in either _last_estimate_ or _new_values_ DONT KNOW IF + // CHANGES ANYTHING + for (const auto& factor : factor_graph) { + gtsam::KeyVector keys = factor->keys(); // use KeyVector directly + for (gtsam::Key key : keys) { + if (!_last_estimate_.exists(key) && !_new_values_.exists(key)) { + // Insert an initial guess + if (gtsam::Symbol(key).chr() == 'x') { + _new_values_.insert(key, gtsam::Pose2(0, 0, 0)); + } else if (gtsam::Symbol(key).chr() == 'l') { + _new_values_.insert(key, gtsam::Point2(0, 0)); + } + } + } + } + + // Add new factors and values to iSAM2 _isam2_.update(this->_new_factors_, this->_new_values_); - RCLCPP_DEBUG(rclcpp::get_logger("slam"), "ISAM2Optimizer - Updating ISAM2"); - this->_new_factors_.resize(0); - this->_new_values_.clear(); + _new_factors_.resize(0); + _new_values_.clear(); + _last_estimate_ = _isam2_.calculateEstimate(); RCLCPP_DEBUG(rclcpp::get_logger("slam"), "ISAM2Optimizer - Optimization complete"); return _last_estimate_; -} \ No newline at end of file +} diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp index ca737c7e0..5e11b8cff 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp @@ -2,45 +2,60 @@ #include "slam_solver/graph_slam_solver/utils.hpp" -PoseUpdater::PoseUpdater([[maybe_unused]] const SLAMParameters& params) { - // Initialize the pose updater with parameters from SLAMParameters - this->_last_pose_ = Eigen::Vector3d::Zero(); - this->_last_graphed_pose_ = Eigen::Vector3d::Zero(); - this->_received_first_motion_data_ = false; - this->_last_pose_update_ = rclcpp::Time(0); - this->_new_pose_from_graph_ = false; - this->_last_pose_covariance_ = Eigen::Matrix3d::Zero(); +PoseUpdater::PoseUpdater(const SLAMParameters& params) + : _pose_buffer_(params.max_pose_history_updater) { + _last_pose_ = Eigen::Vector3d::Zero(); + _last_graphed_pose_ = Eigen::Vector3d::Zero(); + _last_pose_covariance_ = Eigen::Matrix3d::Zero(); + _last_pose_update_ = rclcpp::Time(0); } -PoseUpdater::PoseUpdater(const PoseUpdater& other) { - this->_last_pose_ = other._last_pose_; - this->_last_graphed_pose_ = other._last_graphed_pose_; - this->_last_pose_update_ = other._last_pose_update_; - this->_new_pose_from_graph_ = other._new_pose_from_graph_; - this->_last_pose_covariance_ = other._last_pose_covariance_; - this->_received_first_motion_data_ = other._received_first_motion_data_; +PoseUpdater::PoseUpdater(const PoseUpdater& other) : _pose_buffer_(other._pose_buffer_) { + _last_pose_ = other._last_pose_; + _last_graphed_pose_ = other._last_graphed_pose_; + _last_pose_covariance_ = other._last_pose_covariance_; + _last_pose_update_ = other._last_pose_update_; + _received_first_motion_data_ = other._received_first_motion_data_; + _new_pose_from_graph_ = other._new_pose_from_graph_; } -PoseUpdater::~PoseUpdater() = default; - PoseUpdater& PoseUpdater::operator=(const PoseUpdater& other) { - if (this == &other) return *this; // Prevent self-assignment + if (this == &other) return *this; - // Copy each member individually - this->_last_pose_ = other._last_pose_; - this->_last_pose_update_ = other._last_pose_update_; - this->_last_graphed_pose_ = other._last_graphed_pose_; - this->_last_pose_covariance_ = other._last_pose_covariance_; - this->_new_pose_from_graph_ = other._new_pose_from_graph_; - this->_received_first_motion_data_ = other._received_first_motion_data_; + _last_pose_ = other._last_pose_; + _last_graphed_pose_ = other._last_graphed_pose_; + _last_pose_covariance_ = other._last_pose_covariance_; + _last_pose_update_ = other._last_pose_update_; + _received_first_motion_data_ = other._received_first_motion_data_; + _new_pose_from_graph_ = other._new_pose_from_graph_; + _pose_buffer_ = other._pose_buffer_; return *this; } +PoseUpdater::~PoseUpdater() = default; + std::shared_ptr PoseUpdater::clone() const { return std::make_shared(*this); } +Eigen::Vector3d PoseUpdater::get_pose_at_timestamp(const rclcpp::Time& ts) const { + if (_pose_buffer_.size() == 0) throw std::out_of_range("Pose buffer is empty"); + + for (size_t i = 0; i < _pose_buffer_.size(); i++) { + const auto& curr = _pose_buffer_.from_end(i); + if (curr.timestamp <= ts) { + if (i == 0) return curr.pose; + const auto& prev = _pose_buffer_.from_end(i - 1); + auto diff_curr = ts.nanoseconds() - curr.timestamp.nanoseconds(); + auto diff_prev = prev.timestamp.nanoseconds() - ts.nanoseconds(); + return (diff_curr < diff_prev) ? curr.pose : prev.pose; + } + } + + return _pose_buffer_.from_end(_pose_buffer_.size() - 1).pose; +} + void PoseUpdater::update_pose(const Eigen::Vector3d& last_pose) { this->_last_pose_ = last_pose; this->_last_graphed_pose_ = last_pose; @@ -51,12 +66,13 @@ void PoseUpdater::update_pose(const Eigen::Vector3d& last_pose) { void PoseUpdater::predict_pose(const MotionData& motion_data, std::shared_ptr motion_model) { if (!this->_received_first_motion_data_) { - this->_last_pose_ = Eigen::Vector3d(0.0, 0.0, 0.0); + this->_last_pose_ = Eigen::Vector3d::Zero(); this->_last_pose_update_ = motion_data.timestamp_; this->_received_first_motion_data_ = true; + _pose_buffer_.push(TimedPose(this->_last_pose_, this->_last_pose_update_)); return; } - this->_last_pose_update_.seconds(), motion_data.timestamp_.seconds(); + double delta = (motion_data.timestamp_ - this->_last_pose_update_).seconds(); RCLCPP_DEBUG(rclcpp::get_logger("slam"), "Delta time: %f", delta); Eigen::Vector3d new_pose = @@ -71,12 +87,14 @@ void PoseUpdater::predict_pose(const MotionData& motion_data, this->get_adjoint_operator_matrix(pose_difference(0), pose_difference(1), pose_difference(2)); this->_last_pose_ = new_pose; + this->_last_pose_update_ = motion_data.timestamp_; + _pose_buffer_.push(TimedPose(this->_last_pose_, this->_last_pose_update_)); // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Last Pose Covariance: \n" // << this->_last_pose_covariance_); this->_last_pose_covariance_ = adjoint_matrix * this->_last_pose_covariance_ * adjoint_matrix.transpose() + - motion_noise; // TODO: to improve further, consider including a noise for the intrinsic error - // of the motion model + motion_noise; // TODO: to improve further, consider including a noise for the intrinsic + // error of the motion model this->_last_pose_update_ = motion_data.timestamp_; this->_new_pose_from_graph_ = true; // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("slam"), "Transposed Jacobian Motion Data: \n" @@ -107,4 +125,4 @@ Eigen::Matrix3d PoseUpdater::get_adjoint_operator_matrix(const double x_translat adjoint_matrix(1, 2) = -x_translation; adjoint_matrix(2, 2) = 1.0; return adjoint_matrix; -} \ No newline at end of file +} diff --git a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp index 701a2bb5c..c79ffd10e 100644 --- a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp +++ b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp @@ -133,7 +133,7 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { common_lib::structures::Cone(2.0, -1.0, "yellow", 1.0, rclcpp::Clock().now())); // Act - solver->add_observations(cones_start); + solver->add_observations(cones_start, rclcpp::Clock().now()); solver->add_velocities(velocities); velocities.timestamp_ += rclcpp::Duration(1, 0); solver->add_velocities(velocities); @@ -147,7 +147,7 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { const std::vector map_before_observations = solver->get_map_estimate(); - solver->add_observations(cones_end); + solver->add_observations(cones_end, rclcpp::Clock().now()); const common_lib::structures::Pose pose_after_observations = solver->get_pose_estimate(); const std::vector map_after_observations = solver->get_map_estimate(); From 6103863baa4cec7bc03cea537ad416ad4a87bad8 Mon Sep 17 00:00:00 2001 From: FSFEUP Date: Fri, 28 Nov 2025 11:23:29 +0000 Subject: [PATCH 06/10] testing --- config/control/vehicle.yaml | 2 +- config/planning/vehicle.yaml | 2 +- config/slam/vehicle.yaml | 2 +- ext/as-integration | 2 +- ext/gtsam | 2 +- kill_nodes.sh | 7 ++++++- 6 files changed, 11 insertions(+), 6 deletions(-) diff --git a/config/control/vehicle.yaml b/config/control/vehicle.yaml index 7e7d76344..5029166ac 100644 --- a/config/control/vehicle.yaml +++ b/config/control/vehicle.yaml @@ -2,7 +2,7 @@ control: control_solver: "decoupled" # Options: "decoupled" lateral_controller: "pure_pursuit" # Options: "pure_pursuit" longitudinal_controller: "pid" # Options: "pid" - pure_pursuit_lookahead_gain: 1.5 + pure_pursuit_lookahead_gain: 1.2 pure_pursuit_lookahead_minimum: 3.0 # Acceleration: 6.0 , Skidpad: 4.0 , Autocross: 4.0 , Trackdrive: 4.0 pure_pursuit_lpf_alpha: 1 # Smoothing factor for the low-pass filter (0 = max smoothing, 1 = no smoothing) pure_pursuit_lpf_initial_value: 0.0 diff --git a/config/planning/vehicle.yaml b/config/planning/vehicle.yaml index 31e4ba0b0..e7476fe1e 100644 --- a/config/planning/vehicle.yaml +++ b/config/planning/vehicle.yaml @@ -30,7 +30,7 @@ planning: smoothing_spline_precision: 10 smoothing_use_path_smoothing: false #---------------------- Velocity Planning (vp_) ----------------------- - vp_minimum_velocity: 3.0 + vp_minimum_velocity: 2.0 vp_braking_acceleration: -1.5 vp_normal_acceleration: 0.9 vp_use_velocity_planning: true diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index f59ae0d8d..2eb6ed700 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -25,7 +25,7 @@ slam: slam_optimization_type: "isam2" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type # Only for pose_updater_name: "difference_based_ready" - slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph + slam_min_pose_difference: 0.15 # Minimum distance between poses to consider adding to factor graph # Only for slam_optimization_mode: "async" slam_optimization_period: 0.25 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added diff --git a/ext/as-integration b/ext/as-integration index 7dc3311d2..52b369f64 160000 --- a/ext/as-integration +++ b/ext/as-integration @@ -1 +1 @@ -Subproject commit 7dc3311d2378cfb9ec2736c83888b72949d199c2 +Subproject commit 52b369f64c6b2899087e81f65f166e597844cdc0 diff --git a/ext/gtsam b/ext/gtsam index c9498fe0c..4f66a491f 160000 --- a/ext/gtsam +++ b/ext/gtsam @@ -1 +1 @@ -Subproject commit c9498fe0c07e7750ceada4d1eb798f8f04ac9a2c +Subproject commit 4f66a491ffc83cf092d0d818b11dc35135521612 diff --git a/kill_nodes.sh b/kill_nodes.sh index 309048447..d8c811d47 100755 --- a/kill_nodes.sh +++ b/kill_nodes.sh @@ -1,6 +1,11 @@ # Keywords to match against running ROS 2 processes NODE_PATTERNS=( - "inspection" + "perception_adapter" + "slam" + "velocity_estimation" + "planning_adapter" + "control_adapter" + "ros_can" ) From 1f13f37333afac11f8af9996ab5e2f47d15993fa Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Mon, 1 Dec 2025 20:00:29 +0000 Subject: [PATCH 07/10] parameters and exec time improvements --- config/slam/pacsim.yaml | 5 +++++ config/slam/vehicle.yaml | 11 ++++++++--- .../src/perception/perception_node.cpp | 3 +-- src/slam/include/ros_node/slam_node.hpp | 1 + .../include/slam_config/general_config.hpp | 5 +++++ src/slam/src/ros_node/slam_node.cpp | 12 ++++++++---- src/slam/src/slam_config/general_config.cpp | 12 ++++++++++++ .../graph_slam_solver/graph_slam_instance.cpp | 18 ++++++++++-------- 8 files changed, 50 insertions(+), 17 deletions(-) diff --git a/config/slam/pacsim.yaml b/config/slam/pacsim.yaml index 959ea6ce1..b2b141dcd 100644 --- a/config/slam/pacsim.yaml +++ b/config/slam/pacsim.yaml @@ -24,6 +24,11 @@ slam: slam_optimization_in_poses: false # If true, the optimization will be done also when poses are added, not only when observations are added slam_optimization_type: "sliding_window_levenberg" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type + # Timestamp Synchronization Dependency + synchronized_timestamps: true # Whether motion and observation data are synchronized timestamps or not + max_pose_history_updater: 30 # Maximum number of past poses to keep for pose updater for observation delay compensation + max_pose_history_graph: 10 # Maximum number of past graphed poses to keep for observation delay compensation + # Only for pose_updater_name: "difference_based_ready" slam_min_pose_difference: 0.3 # Minimum distance between poses to consider adding to factor graph diff --git a/config/slam/vehicle.yaml b/config/slam/vehicle.yaml index 2eb6ed700..744a71176 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -5,8 +5,8 @@ slam: lidar_odometry_topic: "-" # "/fast_limo/state" or "/kiss/odometry" or "" -> topic for pose from lidar odometry receive_lidar_odometry: false # Whether to use lidar odometry topic or not data_association_model_name: "nearest_neighbor" # "maximum_likelihood_md" or "maximum_likelihood_nll" or "nearest_neighbor" or... -> name of the data association model - data_association_limit_distance: 70 # maximum distance to consider a cone for data association - data_association_gate: 1.5 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark + data_association_limit_distance: 40 # maximum distance to consider a cone for data association + data_association_gate: 1.2 # Maximum allowed Malahanobis distance (or euclidean distance for nearest neighbor) between an observation and matched landmark new_landmark_confidence_gate: 0.0 # Minimum confidence to consider an observation as a new landmark in data asociation observation_x_noise: 0.02 # sigma of the noise for cone positions in x (range in Graph SLAM) observation_y_noise: 0.001 # sigma of the noise for cone positions in y (bearing in Graph SLAM) @@ -24,6 +24,11 @@ slam: slam_optimization_in_poses: false # If true, the optimization will be done also when poses are added, not only when observations are added slam_optimization_type: "isam2" # "normal_levenberg" or "isam2" or "sliding_window_levenberg" or ... -> name of the optimization type + # Timestamp Synchronization Dependency + synchronized_timestamps: false # Whether motion and observation data are synchronized timestamps or not + max_pose_history_updater: 30 # Maximum number of past poses to keep for pose updater buffer for observation delay compensation + max_pose_history_graph: 7 # Maximum number of past graphed poses to keep in instance buffer for observation delay compensation + # Only for pose_updater_name: "difference_based_ready" slam_min_pose_difference: 0.15 # Minimum distance between poses to consider adding to factor graph @@ -31,7 +36,7 @@ slam: slam_optimization_period: 0.25 # Time between optimization steps in seconds, 0.0 means optimization every time observations are added # Only for slam_optimization_type: "isam2" - slam_isam2_relinearize_threshold: 0.05 # Threshold for relinearizing the graph in isam2 + slam_isam2_relinearize_threshold: 0.1 # Threshold for relinearizing the graph in isam2 slam_isam2_relinearize_skip: 5 # Number of steps to skip before relinearizing the graph in isam2 slam_isam2_factorization: "QR" # "QR" or "CHOLESKY" -> Factorization method for isam2 diff --git a/src/perception/src/perception/perception_node.cpp b/src/perception/src/perception/perception_node.cpp index 2e938ae5f..2fa2d2f11 100644 --- a/src/perception/src/perception/perception_node.cpp +++ b/src/perception/src/perception/perception_node.cpp @@ -245,8 +245,7 @@ Perception::Perception(const PerceptionParameters& params) void Perception::point_cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { // Get start perception time // rclcpp::Time pcl_time = msg->header.stamp; - rclcpp::Time pcl_time = - this->now() - rclcpp::Duration::from_seconds(0.008); // approximate time delay + rclcpp::Time pcl_time = this->now(); // Reset lidar off timer this->lidar_off_timer_ = this->create_wall_timer( diff --git a/src/slam/include/ros_node/slam_node.hpp b/src/slam/include/ros_node/slam_node.hpp index c0f560ec3..841316d96 100644 --- a/src/slam/include/ros_node/slam_node.hpp +++ b/src/slam/include/ros_node/slam_node.hpp @@ -70,6 +70,7 @@ class SLAMNode : public rclcpp::Node { bool _go_ = true; /// flag to start the mission std::string _adapter_name_; std::string _vehicle_frame_id_; ///< Frame id of the vehicle for the transform + SLAMParameters _params_; /** * @brief Callback that updates everytime information diff --git a/src/slam/include/slam_config/general_config.hpp b/src/slam/include/slam_config/general_config.hpp index 9c5231470..df4042990 100644 --- a/src/slam/include/slam_config/general_config.hpp +++ b/src/slam/include/slam_config/general_config.hpp @@ -17,6 +17,8 @@ struct SLAMParameters { // responsible for keeping pose estimate int max_pose_history_updater = 30; // Maximum number of poses to keep in the pose buffer int max_pose_history_graph = 50; // Maximum number of graphed poses to keep in the graph buffer + bool synchronized_timestamps = + true; // Whether motion and observation data are synchronized timestamps or not std::string data_association_model_name_ = "nearest_neighbor"; std::string lidar_odometry_topic_ = "/fast_limo/state"; // Topic for pose from lidar odometry bool receive_lidar_odometry_ = false; // Whether to use lidar odometry topic or not @@ -130,6 +132,9 @@ struct SLAMParameters { << ", minimum_frequency_of_detections_: " << params.minimum_frequency_of_detections_ << ", threshold_dist: " << params.threshold_dist << ", first_x_cones: " << params.first_x_cones << ", border_width: " << params.border_width + << ", synchronized_timestamps: " << params.synchronized_timestamps + << ", max_pose_history_updater: " << params.max_pose_history_updater + << ", max_pose_history_graph: " << params.max_pose_history_graph << ", minimum_confidence: " << params.minimum_confidence << "}"; return os; } diff --git a/src/slam/src/ros_node/slam_node.cpp b/src/slam/src/ros_node/slam_node.cpp index 13b8d6c65..5cddb1ef1 100644 --- a/src/slam/src/ros_node/slam_node.cpp +++ b/src/slam/src/ros_node/slam_node.cpp @@ -18,7 +18,7 @@ /*---------------------- Constructor --------------------*/ -SLAMNode::SLAMNode(const SLAMParameters ¶ms) : Node("slam") { +SLAMNode::SLAMNode(const SLAMParameters ¶ms) : Node("slam"), _params_(params) { // Print parameters RCLCPP_INFO_STREAM(this->get_logger(), "SLAM Node parameters:" << params); // Initialize the models @@ -110,7 +110,9 @@ void SLAMNode::init() { void SLAMNode::_perception_subscription_callback( const custom_interfaces::msg::PerceptionOutput &msg) { auto const &cone_array = msg.cones.cone_array; - rclcpp::Time const cones_time = msg.header.stamp; + + rclcpp::Time const cones_time = + _params_.synchronized_timestamps ? rclcpp::Time(msg.header.stamp) : this->get_clock()->now(); RCLCPP_DEBUG(this->get_logger(), "SUB - Perception: %ld cones. Mission: %d", cone_array.size(), static_cast(this->_mission_)); @@ -172,8 +174,10 @@ void SLAMNode::_velocities_subscription_callback(const custom_interfaces::msg::V msg.velocity_x, msg.velocity_y, msg.angular_velocity, msg.covariance[0], msg.covariance[4], msg.covariance[8], msg.header.stamp); - // this->_vehicle_state_velocities_.timestamp_ = this->get_clock()->now() + - // rclcpp::Duration::from_seconds(0.003); + if (!this->_params_.synchronized_timestamps || 1) { + // If timestamps are not synchronized, set the timestamp to now + this->_vehicle_state_velocities_.timestamp_ = this->get_clock()->now(); + } solver_ptr->add_velocities(this->_vehicle_state_velocities_); } this->_vehicle_pose_ = this->_slam_solver_->get_pose_estimate(); diff --git a/src/slam/src/slam_config/general_config.cpp b/src/slam/src/slam_config/general_config.cpp index 1adb46cf3..cb87f07b8 100644 --- a/src/slam/src/slam_config/general_config.cpp +++ b/src/slam/src/slam_config/general_config.cpp @@ -44,6 +44,10 @@ SLAMParameters::SLAMParameters(const SLAMParameters ¶ms) { first_x_cones = params.first_x_cones; border_width = params.border_width; minimum_confidence = params.minimum_confidence; + + synchronized_timestamps = params.synchronized_timestamps; + max_pose_history_updater = params.max_pose_history_updater; + max_pose_history_graph = params.max_pose_history_graph; } SLAMParameters &SLAMParameters::operator=(const SLAMParameters &other) { @@ -87,6 +91,10 @@ SLAMParameters &SLAMParameters::operator=(const SLAMParameters &other) { first_x_cones = other.first_x_cones; border_width = other.border_width; minimum_confidence = other.minimum_confidence; + + synchronized_timestamps = other.synchronized_timestamps; + max_pose_history_updater = other.max_pose_history_updater; + max_pose_history_graph = other.max_pose_history_graph; } return *this; } @@ -152,5 +160,9 @@ std::string SLAMParameters::load_config() { this->first_x_cones = slam_config["slam"]["first_x_cones"].as(); this->border_width = slam_config["slam"]["border_width"].as(); this->minimum_confidence = slam_config["slam"]["minimum_confidence"].as(); + + this->synchronized_timestamps = slam_config["slam"]["synchronized_timestamps"].as(); + this->max_pose_history_updater = slam_config["slam"]["max_pose_history_updater"].as(); + this->max_pose_history_graph = slam_config["slam"]["max_pose_history_graph"].as(); return adapter; } \ No newline at end of file diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp index 7541c186a..ce5095ada 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp @@ -195,6 +195,8 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ Eigen::VectorXi& associations = *(observation_data.associations_); Eigen::VectorXd& observations_global = *(observation_data.observations_global_); bool new_observation_factors = false; + unsigned int pose_id_at_observation_time = + this->get_landmark_id_at_time(observation_data.timestamp_); for (unsigned int i = 0; i < observations.size() / 2; i++) { gtsam::Point2 landmark; gtsam::Symbol landmark_symbol; @@ -231,14 +233,14 @@ void GraphSLAMInstance::process_observations(const ObservationData& observation_ if (const auto optimizer_ptr = std::dynamic_pointer_cast(this->_optimizer_)) { optimizer_ptr->_new_factors_.add(gtsam::BearingRangeFactor( - gtsam::Symbol('x', this->get_landmark_id_at_time(observation_data.timestamp_)), - landmark_symbol, observation_rotation, observation_cylindrical(0), + gtsam::Symbol('x', pose_id_at_observation_time), landmark_symbol, observation_rotation, + observation_cylindrical(0), observation_noise)); // Again, just for ISAM2, because // it's incremental } this->_factor_graph_.add(gtsam::BearingRangeFactor( - gtsam::Symbol('x', this->get_landmark_id_at_time(observation_data.timestamp_)), - landmark_symbol, observation_rotation, observation_cylindrical(0), observation_noise)); + gtsam::Symbol('x', pose_id_at_observation_time), landmark_symbol, observation_rotation, + observation_cylindrical(0), observation_noise)); } this->_new_pose_node_ = !new_observation_factors; this->_new_observation_factors_ = new_observation_factors; @@ -308,12 +310,12 @@ unsigned int GraphSLAMInstance::get_landmark_id_at_time(const rclcpp::Time& time const TimedPose& curr = _pose_timestamps_.from_end(i); if (curr.timestamp <= timestamp) { - if (i == 0) return curr.pose_id; // AVoid out-of-bounds - const TimedPose& older = _pose_timestamps_.from_end(i - 1); + if (i == 0) return curr.pose_id; // Avoid out-of-bounds + const TimedPose& next = _pose_timestamps_.from_end(i - 1); auto diff_curr = timestamp.nanoseconds() - curr.timestamp.nanoseconds(); - auto diff_older = older.timestamp.nanoseconds() - timestamp.nanoseconds(); - return (diff_curr <= diff_older) ? curr.pose_id : older.pose_id; + auto diff_next = next.timestamp.nanoseconds() - timestamp.nanoseconds(); + return (diff_curr <= diff_next) ? curr.pose_id : next.pose_id; } } From 8909db98fb00bc7d80c9805fdea9059ec3a8b4c7 Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Tue, 2 Dec 2025 10:01:20 +0000 Subject: [PATCH 08/10] ensured buffers are always not empty --- .../src/slam_solver/graph_slam_solver/graph_slam_instance.cpp | 2 ++ .../graph_slam_solver/pose_updater/base_pose_updater.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp index ce5095ada..bc66950cb 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/graph_slam_instance.cpp @@ -83,6 +83,7 @@ GraphSLAMInstance::GraphSLAMInstance(const SLAMParameters& params, const gtsam::Pose2 prior_pose(0.0, 0.0, 0.0); // Create initial prior pose at origin, to bind graph const gtsam::Symbol pose_symbol('x', ++(this->_pose_counter_)); + _pose_timestamps_.push(TimedPose{this->_pose_counter_, rclcpp::Time(0)}); const gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.0, 0.0, 0.0)); _factor_graph_.add(gtsam::PriorFactor(pose_symbol, prior_pose, prior_noise)); @@ -257,6 +258,7 @@ void GraphSLAMInstance::load_initial_state(const Eigen::VectorXd& map, const Eig const gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.0, 0.0, 0.0)); _factor_graph_.add(gtsam::PriorFactor(pose_symbol, prior_pose, prior_noise)); + _pose_timestamps_.push(TimedPose{this->_pose_counter_, rclcpp::Time(0)}); _graph_values_ = gtsam::Values(); _graph_values_.insert(pose_symbol, prior_pose); diff --git a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp index 5e11b8cff..c7936d3ae 100644 --- a/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp @@ -8,6 +8,7 @@ PoseUpdater::PoseUpdater(const SLAMParameters& params) _last_graphed_pose_ = Eigen::Vector3d::Zero(); _last_pose_covariance_ = Eigen::Matrix3d::Zero(); _last_pose_update_ = rclcpp::Time(0); + _pose_buffer_.push(TimedPose(_last_pose_, _last_pose_update_)); } PoseUpdater::PoseUpdater(const PoseUpdater& other) : _pose_buffer_(other._pose_buffer_) { From 630541f94db70756cc179eed4adbdc8de9a187df Mon Sep 17 00:00:00 2001 From: Flopes55 Date: Tue, 2 Dec 2025 10:10:38 +0000 Subject: [PATCH 09/10] made dependencies install executable --- src/slam/dependencies_install.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 src/slam/dependencies_install.sh diff --git a/src/slam/dependencies_install.sh b/src/slam/dependencies_install.sh old mode 100644 new mode 100755 From 0b4ac16c6e1cf8fbb1a6d758bc254386929e7955 Mon Sep 17 00:00:00 2001 From: Database Server Date: Tue, 2 Dec 2025 11:29:56 +0000 Subject: [PATCH 10/10] fixed tests --- .../graph_slam_solver/graph_slam_solver_test.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp index c79ffd10e..6f4bf8777 100644 --- a/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp +++ b/src/slam/test/slam_solver/graph_slam_solver/graph_slam_solver_test.cpp @@ -87,11 +87,12 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { Eigen::VectorXi associations_first = Eigen::VectorXi::Ones(4) * -1; Eigen::VectorXi associations_second = Eigen::VectorXi::Ones(4) * -1; EXPECT_CALL(*mock_motion_model_ptr, get_pose_difference) - .Times(8) + .Times(4) .WillRepeatedly(testing::Return(Eigen::Vector3d(1.1, 0.0, 0.0))); - // EXPECT_CALL(*mock_motion_model_ptr, get_jacobian_motion_data) - // .Times(4) - // .WillRepeatedly(testing::Return(Eigen::Matrix3d::Identity() * 0.1)); +EXPECT_CALL(*mock_motion_model_ptr, get_jacobian_motion_data) + .Times(4) + .WillRepeatedly(testing::Return(Eigen::Matrix3d::Identity() * 0.1)); + EXPECT_CALL(*mock_data_association_ptr, associate) .Times(2) .WillOnce(testing::Return(associations_first)) @@ -125,6 +126,7 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { cones_start.push_back(common_lib::structures::Cone(6.0, 1.0, "blue", 1.0, rclcpp::Clock().now())); cones_start.push_back( common_lib::structures::Cone(6.0, -1.0, "yellow", 1.0, rclcpp::Clock().now())); + cones_end.push_back(common_lib::structures::Cone(-1.0, 1.0, "blue", 1.0, rclcpp::Clock().now())); cones_end.push_back( common_lib::structures::Cone(-1.0, -1.0, "yellow", 1.0, rclcpp::Clock().now())); @@ -156,7 +158,7 @@ TEST_F(GraphSlamSolverTest, MotionAndObservation) { EXPECT_NEAR(pose_before_observations.position.x, 4.4, 0.5); EXPECT_NEAR(pose_after_observations.position.x, 4.4, 0.2); EXPECT_EQ(map_before_observations.size(), 4); - EXPECT_EQ(map_after_observations.size(), 4); + EXPECT_EQ(map_after_observations.size(), 8); EXPECT_NEAR(map_before_observations[0].position.x, 3.0, 0.2); EXPECT_NEAR(map_before_observations[1].position.x, 3.0, 0.2); EXPECT_NEAR(map_before_observations[2].position.x, 6.0, 0.2);