diff --git a/src/motion_lib/include/motion_lib/vel_process_model/kinematic_bicycle.hpp b/cone_viz/cone_viz/__init__.py old mode 100755 new mode 100644 similarity index 100% rename from src/motion_lib/include/motion_lib/vel_process_model/kinematic_bicycle.hpp rename to cone_viz/cone_viz/__init__.py 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' 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/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/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/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/pacsim.yaml b/config/slam/pacsim.yaml index 04cc7c051..b2b141dcd 100644 --- a/config/slam/pacsim.yaml +++ b/config/slam/pacsim.yaml @@ -1,91 +1,54 @@ 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 + + # 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 # 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..744a71176 100644 --- a/config/slam/vehicle.yaml +++ b/config/slam/vehicle.yaml @@ -1,34 +1,54 @@ 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 - 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 + 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) + 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 + + # 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 # 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.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 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/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" ) 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..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 @@ -8,13 +8,14 @@ #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 { 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 gear_ratio = 4; @@ -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/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/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..0487a61f4 100644 --- a/src/motion_lib/CMakeLists.txt +++ b/src/motion_lib/CMakeLists.txt @@ -28,6 +28,8 @@ set(IMPLEMENTATION_FILES src/v2p_models/constant_velocity_model.cpp src/v2p_models/base_v2p_motion_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 @@ -67,6 +69,7 @@ if(BUILD_TESTING) test/main.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/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/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/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/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/tire_model/pacejka_combined_slip.cpp b/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp index 022c8db33..e78597085 100644 --- a/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp +++ b/src/motion_lib/src/tire_model/pacejka_combined_slip.cpp @@ -8,8 +8,8 @@ std::pair PacejkaCombinedSlip::tire_forces(double slip_angle, do 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)))); + 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; @@ -17,8 +17,8 @@ std::pair PacejkaCombinedSlip::tire_forces(double slip_angle, do 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)))); + 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)); 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/motion_lib/test/s2v_models/bicycle_model_test.cpp b/src/motion_lib/test/s2v_models/bicycle_model_test.cpp new file mode 100644 index 000000000..23519535b --- /dev/null +++ b/src/motion_lib/test/s2v_models/bicycle_model_test.cpp @@ -0,0 +1,248 @@ +#include "motion_lib/s2v_model/bicycle_model.hpp" + +#include + +#include + +/* ----------------------- 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(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; + BicycleModel bicycle_model = BicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); // Example velocities + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(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(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; + BicycleModel bicycle_model = BicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(-10.0, 2.0, 0.1); // Example velocities + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(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(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; + BicycleModel bicycle_model = BicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(0.0, -2.0, -0.1); // Example velocities + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(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(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; + BicycleModel bicycle_model = BicycleModel(car_parameters_); + Eigen::Vector3d cg_velocities(0.0, 0.0, 0.0); // Example velocities + Eigen::VectorXd observations = bicycle_model.cg_velocity_to_wheels(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(BicycleModelTest, 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 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.jacobian_cg_velocity_to_wheels(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(BicycleModelTest, 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 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.jacobian_cg_velocity_to_wheels(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/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/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..2fa2d2f11 100644 --- a/src/perception/src/perception/perception_node.cpp +++ b/src/perception/src/perception/perception_node.cpp @@ -243,6 +243,11 @@ 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(); + + // 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 +313,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/perception_sensor_lib/CMakeLists.txt b/src/perception_sensor_lib/CMakeLists.txt index 3a02ac3da..7e22291e8 100644 --- a/src/perception_sensor_lib/CMakeLists.txt +++ b/src/perception_sensor_lib/CMakeLists.txt @@ -22,6 +22,13 @@ foreach(pkg ${PACKAGES}) find_package(${pkg} REQUIRED) endforeach() +# Find PCL +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) if(OpenMP_CXX_FOUND) @@ -31,10 +38,11 @@ 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/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 ) @@ -46,10 +54,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 +83,11 @@ if(BUILD_TESTING) set(TESTFILES test/main.cpp - test/observation_model/slam_observation_model/default_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 test/data_association/maximum_likelihood_md_test.cpp test/landmark_filter/consecutive_counter_filter_test.cpp ) @@ -89,7 +101,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/slam/slam_base_observation_model.hpp similarity index 96% 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/slam/slam_base_observation_model.hpp index 95ba410a7..fed009d55 100644 --- 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/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_observation_model/map.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/map.hpp similarity index 99% rename from src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/map.hpp rename to src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/map.hpp index ad51791df..5c21a7d81 100644 --- 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/map.hpp @@ -21,4 +21,4 @@ const std::map 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_observation_model/no_slip_bicycle_model.hpp b/src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp similarity index 96% rename from src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp rename to src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp index a301d8a84..dc23986df 100644 --- 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/no_slip_bicycle_model.hpp @@ -1,6 +1,6 @@ #pragma once -#include "base_observation_model.hpp" +#include "ve_base_observation_model.hpp" class NoSlipBicycleModel : public VEObservationModel { public: 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/ve_base_observation_model.hpp similarity index 99% rename from src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve_observation_model/base_observation_model.hpp rename to src/perception_sensor_lib/include/perception_sensor_lib/observation_model/ve/ve_base_observation_model.hpp index be2d7643e..98bd1914b 100644 --- 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/ve_base_observation_model.hpp @@ -40,4 +40,4 @@ class VEObservationModel { * @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/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/slam/slam_base_observation_model.cpp similarity index 87% 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/slam/slam_base_observation_model.cpp index 102d72265..8b133eb6d 100644 --- a/src/perception_sensor_lib/src/observation_model/slam_observation_model/default_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/slam_observation_model/default_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_observation_model/no_slip_bicycle_model.cpp b/src/perception_sensor_lib/src/observation_model/ve/no_slip_bicycle_model.cpp similarity index 97% rename from src/perception_sensor_lib/src/observation_model/ve_observation_model/no_slip_bicycle_model.cpp rename to src/perception_sensor_lib/src/observation_model/ve/no_slip_bicycle_model.cpp index 16d4e014b..d79597844 100644 --- 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/no_slip_bicycle_model.cpp @@ -1,4 +1,4 @@ -#include "perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp" +#include "perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp" Eigen::VectorXd NoSlipBicycleModel::expected_observations( const Eigen::VectorXd& cg_velocities) const { 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/slam/slam_base_observation_model_test.cpp similarity index 74% 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/slam/slam_base_observation_model_test.cpp index 0c724f3f1..f282d511d 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/slam/slam_base_observation_model_test.cpp @@ -2,16 +2,16 @@ #include -#include "perception_sensor_lib/observation_model/slam_observation_model/default_observation_model.hpp" +#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_observation_model/no_slip_bicycle_model_test.cpp b/src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp similarity index 98% rename from src/perception_sensor_lib/test/observation_model/ve_observation_model/no_slip_bicycle_model_test.cpp rename to src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp index 3792f08db..97a8d32b3 100644 --- a/src/perception_sensor_lib/test/observation_model/ve_observation_model/no_slip_bicycle_model_test.cpp +++ b/src/perception_sensor_lib/test/observation_model/ve/no_slip_bicycle_model_test.cpp @@ -1,4 +1,4 @@ -#include "perception_sensor_lib/observation_model/ve_observation_model/no_slip_bicycle_model.hpp" +#include "perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp" #include 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/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..841316d96 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,34 @@ 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 + SLAMParameters _params_; /** * @brief Callback that updates everytime information @@ -76,22 +82,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..df4042990 100644 --- a/src/slam/include/slam_config/general_config.hpp +++ b/src/slam/include/slam_config/general_config.hpp @@ -13,40 +13,61 @@ 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 + 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 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 +77,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 +111,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 +121,28 @@ 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 + << ", 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; } + /** + * @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..a9f861865 100644 --- a/src/slam/include/slam_solver/ekf_slam_solver.hpp +++ b/src/slam/include/slam_solver/ekf_slam_solver.hpp @@ -5,12 +5,13 @@ #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/slam/slam_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_; + std::shared_ptr observation_model_; Eigen::VectorXd state_ = Eigen::VectorXd::Zero(3); Eigen::MatrixXd covariance_; Eigen::MatrixXd process_noise_matrix_; @@ -105,27 +106,20 @@ 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 * * @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 @@ -133,7 +127,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..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,12 +17,15 @@ */ 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 _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_ = @@ -32,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; @@ -43,6 +52,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 +77,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 +132,54 @@ 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, const rclcpp::Time& pose_timestamp); /** - * @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); + + 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 */ 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..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 @@ -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,16 +107,36 @@ 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) * * @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 @@ -96,7 +145,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 +157,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 +200,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..9ffd7f011 --- /dev/null +++ b/src/slam/include/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp @@ -0,0 +1,95 @@ +#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" + +/** + * @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: + struct TimedPose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d pose; + rclcpp::Time timestamp; + + 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 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; + + /** + * @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; + + 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_; } + + 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_; } + + 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/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..bc078deaf 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,28 +57,13 @@ 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) * * @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 @@ -87,7 +72,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..5cddb1ef1 100644 --- a/src/slam/src/ros_node/slam_node.cpp +++ b/src/slam/src/ros_node/slam_node.cpp @@ -18,8 +18,9 @@ /*---------------------- 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 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,28 +82,28 @@ 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 --------------------*/ @@ -96,46 +111,31 @@ void SLAMNode::_perception_subscription_callback( const custom_interfaces::msg::PerceptionOutput &msg) { auto const &cone_array = msg.cones.cone_array; + 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_)); + 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; - 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_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)); } - 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->_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(); @@ -156,7 +156,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 +165,50 @@ 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; - - 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(); + 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); + + 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(); - 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]); + + imu_data.timestamp_ = this->get_clock()->now(); + + solver_ptr->add_imu_data(imu_data); + } +} + /*---------------------- Publications --------------------*/ void SLAMNode::_publish_vehicle_pose() { @@ -205,15 +217,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 +238,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 +303,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..cb87f07b8 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,27 @@ 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; + + 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) { @@ -40,12 +55,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 +76,25 @@ 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; + + 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; } @@ -83,6 +116,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 +131,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 +140,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 +151,18 @@ 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(); + + 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/ekf_slam_solver.cpp b/src/slam/src/slam_solver/ekf_slam_solver.cpp index 25d61209b..bacfdbf20 100644 --- a/src/slam/src/slam_solver/ekf_slam_solver.cpp +++ b/src/slam/src/slam_solver/ekf_slam_solver.cpp @@ -17,11 +17,9 @@ 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(); } -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 { @@ -41,7 +39,9 @@ void EKFSLAMSolver::add_motion_prior(const common_lib::structures::Velocities& v 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; Eigen::VectorXd matched_observations(0); @@ -52,10 +52,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 +78,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 +153,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 +171,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..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 @@ -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_; } @@ -76,14 +77,21 @@ 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, 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_)); + _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)); + 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 +107,9 @@ 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_; + _pose_timestamps_ = other._pose_timestamps_; } GraphSLAMInstance& GraphSLAMInstance::operator=(const GraphSLAMInstance& other) { @@ -114,60 +122,73 @@ 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_; + _pose_timestamps_ = other._pose_timestamps_; 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, + 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); - 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_); + + // X means pose node, l means landmark node 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)); + // 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; - 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) { @@ -175,6 +196,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; @@ -188,7 +211,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,19 +229,27 @@ 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', 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->_pose_counter_), landmark_symbol, observation_rotation, + 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; } -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(); @@ -220,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); @@ -238,7 +277,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 +289,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)) { @@ -258,4 +301,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& next = _pose_timestamps_.from_end(i - 1); + + auto diff_curr = timestamp.nanoseconds() - curr.timestamp.nanoseconds(); + auto diff_next = next.timestamp.nanoseconds() - timestamp.nanoseconds(); + return (diff_curr <= diff_next) ? curr.pose_id : next.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 4cf1fc474..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 @@ -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,126 @@ 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->get_last_pose_update()); + + 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 +158,32 @@ 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) { +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)); + // 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,113 +193,82 @@ 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(); + state = this->_graph_slam_instance_->get_state_vector(); + landmarks = state.segment(3, state.size() - 3); + 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(state.head<3>(), observations); + common_lib::maths::local_to_global_coordinates(adjusted_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), 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"), + "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 +283,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->_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 +363,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 +391,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 +401,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..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 @@ -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,63 @@ 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(); }; +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(gtsam::NonlinearFactorGraph& factor_graph, 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)); + 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)); + } + } } } - _isam2_.update(factor_graph, new_values); - factor_graph.resize(0); // Clear the factor graph + // Add new factors and values to iSAM2 + _isam2_.update(this->_new_factors_, this->_new_values_); + _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/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..c7936d3ae --- /dev/null +++ b/src/slam/src/slam_solver/graph_slam_solver/pose_updater/base_pose_updater.cpp @@ -0,0 +1,129 @@ +#include "slam_solver/graph_slam_solver/pose_updater/base_pose_updater.hpp" + +#include "slam_solver/graph_slam_solver/utils.hpp" + +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); + _pose_buffer_.push(TimedPose(_last_pose_, _last_pose_update_)); +} + +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::operator=(const PoseUpdater& other) { + if (this == &other) return *this; + + _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; + 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::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; + } + + 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; + 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 + 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; +} 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..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 @@ -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)); }; @@ -86,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_velocities) - // .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)) @@ -124,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())); @@ -132,21 +135,21 @@ 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_motion_prior(velocities); + solver->add_observations(cones_start, rclcpp::Clock().now()); + 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(); - 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(); @@ -155,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); 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_;